diff --git a/libs/kaptan-drivers/Cargo.toml b/libs/kaptan-drivers/Cargo.toml new file mode 100644 index 0000000..6361f79 --- /dev/null +++ b/libs/kaptan-drivers/Cargo.toml @@ -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" } diff --git a/libs/kaptan-drivers/src/lib.rs b/libs/kaptan-drivers/src/lib.rs new file mode 100644 index 0000000..2ac0658 --- /dev/null +++ b/libs/kaptan-drivers/src/lib.rs @@ -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; diff --git a/libs/kaptan-drivers/src/mpu6000/config.rs b/libs/kaptan-drivers/src/mpu6000/config.rs new file mode 100644 index 0000000..f207fd4 --- /dev/null +++ b/libs/kaptan-drivers/src/mpu6000/config.rs @@ -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, + } + } +} diff --git a/libs/kaptan-drivers/src/mpu6000/error.rs b/libs/kaptan-drivers/src/mpu6000/error.rs new file mode 100644 index 0000000..f263cff --- /dev/null +++ b/libs/kaptan-drivers/src/mpu6000/error.rs @@ -0,0 +1,15 @@ +//! MPU6000 operation errors. + +/// MPU6000 operation failure. +#[derive(Debug, Eq, PartialEq)] +pub enum Error { + /// 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, +} diff --git a/libs/kaptan-drivers/src/mpu6000/mod.rs b/libs/kaptan-drivers/src/mpu6000/mod.rs new file mode 100644 index 0000000..a610301 --- /dev/null +++ b/libs/kaptan-drivers/src/mpu6000/mod.rs @@ -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 { + /// 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), +} + +/// 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, + config: Mpu6000Config, + state: State, + transmit: [u8; TRANSFER_CAPACITY], + receive: [u8; TRANSFER_CAPACITY], +} + +impl Mpu6000 { + /// 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 Mpu6000 { + /// 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> { + 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> { + 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> { + 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 { + 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 { + 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 { + 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> { + 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) + )); + } +} diff --git a/libs/kaptan-drivers/src/mpu6000/registers.rs b/libs/kaptan-drivers/src/mpu6000/registers.rs new file mode 100644 index 0000000..b552921 --- /dev/null +++ b/libs/kaptan-drivers/src/mpu6000/registers.rs @@ -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; diff --git a/libs/kaptan-drivers/src/mpu6000/sample.rs b/libs/kaptan-drivers/src/mpu6000/sample.rs new file mode 100644 index 0000000..a793380 --- /dev/null +++ b/libs/kaptan-drivers/src/mpu6000/sample.rs @@ -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 +}