libs/kaptan-drivers: add MPU6000 state machine

2026-07-19 22:39:55 +03:00
1 parent 1dadce2bd0
commit 2491cc6fb3
@@ -0,0 +1,15 @@
+[package]
+name = "kaptan-drivers"
+edition.workspace = true
+license.workspace = true
+repository.workspace = true
+rust-version.workspace = true
+version.workspace = true
+
+[lints]
+workspace = true
+
+[dependencies]
+kaptan-sensor = { path = "../kaptan-sensor" }
+kaptan-spi = { path = "../kaptan-spi" }
+kaptan-time = { path = "../kaptan-time" }
@@ -0,0 +1,10 @@
+//! Hardware-independent chip protocol drivers.
+//!
+//! Drivers express device register protocols and data conversion through small
+//! bus traits. MCU, pin, DMA, interrupt, and RTIC mechanics belong below this
+//! layer in a HAL or board adapter.
+
+#![no_std]
+
+/// InvenSense MPU6000 six-axis IMU driver.
+pub mod mpu6000;
@@ -0,0 +1,125 @@
+//! MPU6000 full-scale configuration and conversion factors.
+
+/// MPU6000 full-scale configuration.
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+pub struct Mpu6000Config {
+    /// Accelerometer full-scale range.
+    pub accelerometer: AccelerometerRange,
+    /// Gyroscope full-scale range.
+    pub gyroscope: GyroscopeRange,
+    /// Digital low-pass filter configuration.
+    pub digital_low_pass_filter: DigitalLowPassFilter,
+    /// Divider applied to the filter's internal sample rate.
+    pub sample_rate_divider: u8,
+}
+
+impl Default for Mpu6000Config {
+    fn default() -> Self {
+        Self {
+            accelerometer: AccelerometerRange::PlusMinus4G,
+            gyroscope: GyroscopeRange::PlusMinus1000DegreesPerSecond,
+            digital_low_pass_filter: DigitalLowPassFilter::Bandwidth42Hz,
+            sample_rate_divider: 9,
+        }
+    }
+}
+
+/// MPU6000 digital low-pass filter bandwidth selection.
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+pub enum DigitalLowPassFilter {
+    /// 260 Hz bandwidth; internal sample rate is 8 kHz.
+    Bandwidth260Hz,
+    /// 184 Hz bandwidth; internal sample rate is 1 kHz.
+    Bandwidth184Hz,
+    /// 94 Hz bandwidth; internal sample rate is 1 kHz.
+    Bandwidth94Hz,
+    /// 42 Hz bandwidth; internal sample rate is 1 kHz.
+    Bandwidth42Hz,
+    /// 21 Hz bandwidth; internal sample rate is 1 kHz.
+    Bandwidth21Hz,
+    /// 10 Hz bandwidth; internal sample rate is 1 kHz.
+    Bandwidth10Hz,
+    /// 5 Hz bandwidth; internal sample rate is 1 kHz.
+    Bandwidth5Hz,
+}
+
+impl DigitalLowPassFilter {
+    pub(crate) const fn register_value(self) -> u8 {
+        match self {
+            Self::Bandwidth260Hz => 0,
+            Self::Bandwidth184Hz => 1,
+            Self::Bandwidth94Hz => 2,
+            Self::Bandwidth42Hz => 3,
+            Self::Bandwidth21Hz => 4,
+            Self::Bandwidth10Hz => 5,
+            Self::Bandwidth5Hz => 6,
+        }
+    }
+}
+
+/// MPU6000 accelerometer full-scale range.
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+pub enum AccelerometerRange {
+    /// ±2 g.
+    PlusMinus2G,
+    /// ±4 g.
+    PlusMinus4G,
+    /// ±8 g.
+    PlusMinus8G,
+    /// ±16 g.
+    PlusMinus16G,
+}
+
+impl AccelerometerRange {
+    pub(crate) const fn register_value(self) -> u8 {
+        match self {
+            Self::PlusMinus2G => 0x00,
+            Self::PlusMinus4G => 0x08,
+            Self::PlusMinus8G => 0x10,
+            Self::PlusMinus16G => 0x18,
+        }
+    }
+
+    pub(crate) const fn counts_per_g(self) -> i32 {
+        match self {
+            Self::PlusMinus2G => 16_384,
+            Self::PlusMinus4G => 8_192,
+            Self::PlusMinus8G => 4_096,
+            Self::PlusMinus16G => 2_048,
+        }
+    }
+}
+
+/// MPU6000 gyroscope full-scale range.
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+pub enum GyroscopeRange {
+    /// ±250 degrees per second.
+    PlusMinus250DegreesPerSecond,
+    /// ±500 degrees per second.
+    PlusMinus500DegreesPerSecond,
+    /// ±1000 degrees per second.
+    PlusMinus1000DegreesPerSecond,
+    /// ±2000 degrees per second.
+    PlusMinus2000DegreesPerSecond,
+}
+
+impl GyroscopeRange {
+    pub(crate) const fn register_value(self) -> u8 {
+        match self {
+            Self::PlusMinus250DegreesPerSecond => 0x00,
+            Self::PlusMinus500DegreesPerSecond => 0x08,
+            Self::PlusMinus1000DegreesPerSecond => 0x10,
+            Self::PlusMinus2000DegreesPerSecond => 0x18,
+        }
+    }
+
+    /// Divisor for $\mathrm{mrad/s} = raw \times 17453 / divisor$.
+    pub(crate) const fn milliradians_per_second_divisor(self) -> i32 {
+        match self {
+            Self::PlusMinus250DegreesPerSecond => 131_000,
+            Self::PlusMinus500DegreesPerSecond => 65_500,
+            Self::PlusMinus1000DegreesPerSecond => 32_800,
+            Self::PlusMinus2000DegreesPerSecond => 16_400,
+        }
+    }
+}
@@ -0,0 +1,15 @@
+//! MPU6000 operation errors.
+
+/// MPU6000 operation failure.
+#[derive(Debug, Eq, PartialEq)]
+pub enum Error<BusError> {
+    /// The selected SPI route rejected a transfer submission.
+    Start(BusError),
+    /// The responding device does not identify as an MPU6000.
+    UnexpectedIdentity(u8),
+    /// An acquisition request arrived before initialization completed or while
+    /// another transfer still owned the selected route.
+    NotReady,
+    /// The selected SPI route reported a terminal transfer fault.
+    TransferFault,
+}
@@ -0,0 +1,361 @@
+//! MPU6000 register protocol and sample driver.
+
+mod config;
+mod error;
+mod registers;
+mod sample;
+
+use kaptan_sensor::ImuSnapshot;
+use kaptan_spi::{SpiCompletion, SpiDevice};
+use kaptan_time::Timestamp;
+
+pub use config::{AccelerometerRange, DigitalLowPassFilter, GyroscopeRange, Mpu6000Config};
+pub use error::Error;
+
+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,
+};
+
+const TRANSFER_CAPACITY: usize = SAMPLE_BYTES + 1;
+const CONFIGURATION: [(u8, u8); 9] = [
+    (PWR_MGMT_1, CLOCK_SOURCE_PLL_X_GYRO),
+    (USER_CTRL, I2C_INTERFACE_DISABLE),
+    (PWR_MGMT_2, ENABLE_ALL_SENSOR_AXES),
+    (CONFIG, 0),
+    (SMPLRT_DIV, 0),
+    (ACCEL_CONFIG, 0),
+    (GYRO_CONFIG, 0),
+    (INT_PIN_CFG, INTERRUPT_CLEAR_ON_ANY_READ),
+    (INT_ENABLE, DATA_READY_INTERRUPT),
+];
+
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+enum State {
+    Dormant,
+    Resetting,
+    WaitingForReset,
+    ReadingIdentity,
+    WritingConfiguration(usize),
+    Ready,
+    ReadingSample { timestamp: Timestamp },
+}
+
+/// Observable outcome of one SPI terminal event.
+#[derive(Debug, Eq, PartialEq)]
+pub enum Mpu6000Event<BusError> {
+    /// The selected SPI interrupt had no terminal transfer event.
+    NotPending,
+    /// One initialization transfer completed; initialization continues.
+    Initializing,
+    /// The MPU6000 identity and configuration sequence completed.
+    Initialized,
+    /// One coherent IMU sample completed.
+    Sample(ImuSnapshot),
+    /// Initialization or acquisition failed.
+    Fault(Error<BusError>),
+}
+
+/// Configured MPU6000 driver.
+///
+/// The driver owns fixed transfer buffers and advances only from external
+/// interrupt events. It never waits for SPI status flags.
+pub struct Mpu6000<Bus> {
+    bus: Bus,
+    config: Mpu6000Config,
+    state: State,
+    transmit: [u8; TRANSFER_CAPACITY],
+    receive: [u8; TRANSFER_CAPACITY],
+}
+
+impl<Bus> Mpu6000<Bus> {
+    /// Creates an inactive MPU6000 state machine.
+    pub const fn new(bus: Bus, config: Mpu6000Config) -> Self {
+        Self {
+            bus,
+            config,
+            state: State::Dormant,
+            transmit: [0; TRANSFER_CAPACITY],
+            receive: [0; TRANSFER_CAPACITY],
+        }
+    }
+}
+
+impl<Bus: SpiDevice> Mpu6000<Bus> {
+    /// Starts a device reset without waiting for SPI.
+    ///
+    /// The caller must subsequently call [`Self::advance_initialization`] only
+    /// after the board-selected reset-settling interval has elapsed.
+    pub fn begin_initialization(&mut self) -> Result<(), Error<Bus::StartError>> {
+        if self.state != State::Dormant {
+            return Err(Error::NotReady);
+        }
+
+        self.prepare_write(PWR_MGMT_1, DEVICE_RESET);
+        self.submit(2)?;
+        self.state = State::Resetting;
+        Ok(())
+    }
+
+    /// Starts identity verification after the required reset-settling interval.
+    ///
+    /// This operation only submits the next asynchronous SPI transfer; it
+    /// never waits for a peripheral or sensor status flag.
+    pub fn advance_initialization(&mut self) -> Result<(), Error<Bus::StartError>> {
+        if self.state != State::WaitingForReset {
+            return Ok(());
+        }
+
+        self.prepare_read(WHO_AM_I, 2);
+        if let Err(error) = self.submit(2) {
+            self.state = State::Dormant;
+            return Err(error);
+        }
+        self.state = State::ReadingIdentity;
+        Ok(())
+    }
+
+    /// Starts one fourteen-byte sensor read when initialization is complete.
+    pub fn request_sample(&mut self, timestamp: Timestamp) -> Result<(), Error<Bus::StartError>> {
+        if self.state != State::Ready {
+            return Err(Error::NotReady);
+        }
+
+        self.prepare_read(ACCEL_XOUT_H, TRANSFER_CAPACITY);
+        self.submit(TRANSFER_CAPACITY)?;
+        self.state = State::ReadingSample { timestamp };
+        Ok(())
+    }
+
+    /// Advances the state machine from an already-acknowledged DMA terminal event.
+    pub fn on_transfer_complete(
+        &mut self,
+        completion: SpiCompletion,
+    ) -> Mpu6000Event<Bus::StartError> {
+        self.bus.complete_transfer(completion);
+        match completion {
+            SpiCompletion::NotPending => Mpu6000Event::NotPending,
+            SpiCompletion::TransferFault => {
+                self.state = State::Dormant;
+                Mpu6000Event::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) -> Mpu6000Event<Bus::StartError> {
+        match self.state {
+            State::Dormant => Mpu6000Event::NotPending,
+            State::Resetting => {
+                self.state = State::WaitingForReset;
+                Mpu6000Event::Initializing
+            }
+            State::WaitingForReset => Mpu6000Event::NotPending,
+            State::ReadingIdentity => {
+                let identity = self.receive[1];
+                if identity != EXPECTED_WHO_AM_I {
+                    self.state = State::Dormant;
+                    return Mpu6000Event::Fault(Error::UnexpectedIdentity(identity));
+                }
+
+                self.start_configuration(0)
+            }
+            State::WritingConfiguration(index) => {
+                let next = index + 1;
+                if next == CONFIGURATION.len() {
+                    self.state = State::Ready;
+                    Mpu6000Event::Initialized
+                } else {
+                    self.start_configuration(next)
+                }
+            }
+            State::Ready => Mpu6000Event::NotPending,
+            State::ReadingSample { timestamp } => {
+                let mut bytes = [0; SAMPLE_BYTES];
+                bytes.copy_from_slice(&self.receive[1..]);
+                self.state = State::Ready;
+                Mpu6000Event::Sample(sample::decode(&bytes, timestamp, self.config))
+            }
+        }
+    }
+
+    fn start_configuration(&mut self, index: usize) -> Mpu6000Event<Bus::StartError> {
+        let (register, value) = CONFIGURATION[index];
+        let value = match register {
+            CONFIG => self.config.digital_low_pass_filter.register_value(),
+            SMPLRT_DIV => self.config.sample_rate_divider,
+            ACCEL_CONFIG => self.config.accelerometer.register_value(),
+            GYRO_CONFIG => self.config.gyroscope.register_value(),
+            _ => value,
+        };
+        self.prepare_write(register, value);
+        match self.submit(2) {
+            Ok(()) => {
+                self.state = State::WritingConfiguration(index);
+                Mpu6000Event::Initializing
+            }
+            Err(error) => {
+                self.state = State::Dormant;
+                Mpu6000Event::Fault(error)
+            }
+        }
+    }
+
+    fn prepare_read(&mut self, register: u8, length: usize) {
+        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, Mpu6000, Mpu6000Config, Mpu6000Event, TRANSFER_CAPACITY};
+    use kaptan_spi::{SpiCompletion, SpiDevice};
+    use kaptan_time::Timestamp;
+
+    #[derive(Debug, Eq, PartialEq)]
+    enum FakeError {
+        Start,
+    }
+
+    struct FakeBus {
+        receive: [u8; TRANSFER_CAPACITY],
+        length: usize,
+        fail_start: bool,
+    }
+
+    impl FakeBus {
+        fn ready() -> Self {
+            Self {
+                receive: [0; TRANSFER_CAPACITY],
+                length: 0,
+                fail_start: false,
+            }
+        }
+    }
+
+    impl SpiDevice for FakeBus {
+        type StartError = FakeError;
+
+        fn start_transfer(&mut self, _: &[u8], receive: &mut [u8]) -> Result<(), Self::StartError> {
+            if self.fail_start {
+                return Err(FakeError::Start);
+            }
+
+            self.length = receive.len();
+            receive.copy_from_slice(&self.receive[..self.length]);
+            Ok(())
+        }
+
+        fn complete_transfer(&mut self, _: SpiCompletion) {}
+    }
+
+    #[test]
+    fn configures_a_verified_device_without_waiting() {
+        let mut bus = FakeBus::ready();
+        bus.receive[1] = 0x68;
+        let mut driver = Mpu6000::new(bus, Mpu6000Config::default());
+
+        driver.begin_initialization().unwrap();
+        assert_eq!(
+            driver.on_transfer_complete(SpiCompletion::Complete),
+            Mpu6000Event::Initializing
+        );
+        driver.advance_initialization().unwrap();
+        for _ in 0..9 {
+            assert_eq!(
+                driver.on_transfer_complete(SpiCompletion::Complete),
+                Mpu6000Event::Initializing
+            );
+        }
+        assert_eq!(
+            driver.on_transfer_complete(SpiCompletion::Complete),
+            Mpu6000Event::Initialized
+        );
+        assert!(driver.is_ready());
+    }
+
+    #[test]
+    fn rejects_an_unexpected_identity() {
+        let mut bus = FakeBus::ready();
+        bus.receive[1] = 0x70;
+        let mut driver = Mpu6000::new(bus, Mpu6000Config::default());
+
+        driver.begin_initialization().unwrap();
+        let _ = driver.on_transfer_complete(SpiCompletion::Complete);
+        driver.advance_initialization().unwrap();
+        assert!(matches!(
+            driver.on_transfer_complete(SpiCompletion::Complete),
+            Mpu6000Event::Fault(Error::UnexpectedIdentity(0x70))
+        ));
+    }
+
+    #[test]
+    fn converts_a_complete_burst_to_core_units() {
+        let burst = [
+            0x20, 0x00, 0xE0, 0x00, 0x00, 0x00, // accel: 1g, -1g, 0g
+            0x00, 0x00, // 36.53 C
+            0x40, 0x10, 0xBF, 0xF0, 0x00, 0x00, // gyro: 500, -500, 0 dps
+        ];
+        let mut bus = FakeBus::ready();
+        bus.receive[1] = 0x68;
+        let mut driver = Mpu6000::new(bus, Mpu6000Config::default());
+
+        driver.begin_initialization().unwrap();
+        let _ = driver.on_transfer_complete(SpiCompletion::Complete);
+        driver.advance_initialization().unwrap();
+        for _ in 0..9 {
+            let _ = driver.on_transfer_complete(SpiCompletion::Complete);
+        }
+        let _ = driver.on_transfer_complete(SpiCompletion::Complete);
+        driver.bus.receive[1..].copy_from_slice(&burst);
+        driver
+            .request_sample(Timestamp::from_microseconds(42))
+            .unwrap();
+        let Mpu6000Event::Sample(sample) = driver.on_transfer_complete(SpiCompletion::Complete)
+        else {
+            panic!("sample transfer must complete");
+        };
+
+        assert_eq!(sample.acceleration_milligravity, [1_000, -1_000, 0]);
+        assert_eq!(
+            sample.angular_rate_milliradians_per_second,
+            [8_726, -8_726, 0]
+        );
+        assert_eq!(sample.temperature_centidegrees_celsius, 3_653);
+    }
+
+    #[test]
+    fn reports_a_route_transfer_fault() {
+        let mut bus = FakeBus::ready();
+        bus.receive[1] = 0x68;
+        let mut driver = Mpu6000::new(bus, Mpu6000Config::default());
+
+        driver.begin_initialization().unwrap();
+        assert!(matches!(
+            driver.on_transfer_complete(SpiCompletion::TransferFault),
+            Mpu6000Event::Fault(Error::TransferFault)
+        ));
+    }
+}
@@ -0,0 +1,22 @@
+//! MPU6000 register addresses and fixed protocol values.
+
+pub(crate) const WHO_AM_I: u8 = 0x75;
+pub(crate) const EXPECTED_WHO_AM_I: u8 = 0x68;
+pub(crate) const PWR_MGMT_1: u8 = 0x6B;
+pub(crate) const PWR_MGMT_2: u8 = 0x6C;
+pub(crate) const USER_CTRL: u8 = 0x6A;
+pub(crate) const SMPLRT_DIV: u8 = 0x19;
+pub(crate) const CONFIG: u8 = 0x1A;
+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 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 SAMPLE_BYTES: usize = 14;
@@ -0,0 +1,43 @@
+//! MPU6000 burst-sample decoding.
+
+use kaptan_sensor::ImuSnapshot;
+use kaptan_time::Timestamp;
+
+use super::{config::Mpu6000Config, registers::SAMPLE_BYTES};
+
+pub(crate) fn decode(
+    bytes: &[u8; SAMPLE_BYTES],
+    timestamp: Timestamp,
+    config: Mpu6000Config,
+) -> ImuSnapshot {
+    ImuSnapshot::new(
+        timestamp,
+        [
+            scale_acceleration(read_i16(bytes, 0), config),
+            scale_acceleration(read_i16(bytes, 2), config),
+            scale_acceleration(read_i16(bytes, 4), config),
+        ],
+        [
+            scale_angular_rate(read_i16(bytes, 8), config),
+            scale_angular_rate(read_i16(bytes, 10), config),
+            scale_angular_rate(read_i16(bytes, 12), config),
+        ],
+        scale_temperature(read_i16(bytes, 6)),
+    )
+}
+
+const fn read_i16(bytes: &[u8; SAMPLE_BYTES], offset: usize) -> i16 {
+    i16::from_be_bytes([bytes[offset], bytes[offset + 1]])
+}
+
+const fn scale_acceleration(raw: i16, config: Mpu6000Config) -> i16 {
+    ((raw as i32 * 1_000) / config.accelerometer.counts_per_g()) as i16
+}
+
+const fn scale_angular_rate(raw: i16, config: Mpu6000Config) -> i16 {
+    ((raw as i32 * 17_453) / config.gyroscope.milliradians_per_second_divisor()) as i16
+}
+
+const fn scale_temperature(raw: i16) -> i16 {
+    ((raw as i32 * 100) / 340 + 3_653) as i16
+}