diff --git a/libs/kaptan-drivers/src/icm42688/error.rs b/libs/kaptan-drivers/src/icm42688/error.rs new file mode 100644 index 0000000..bd7868c --- /dev/null +++ b/libs/kaptan-drivers/src/icm42688/error.rs @@ -0,0 +1,15 @@ +//! ICM-42688-P operation errors. + +/// ICM-42688-P 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 ICM-42688-P. + 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/icm42688/mod.rs b/libs/kaptan-drivers/src/icm42688/mod.rs new file mode 100644 index 0000000..48b32ce --- /dev/null +++ b/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 { + /// 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), +} + +/// 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, + state: State, + transmit: [u8; TRANSFER_CAPACITY], + receive: [u8; TRANSFER_CAPACITY], +} + +impl Icm42688 { + /// 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 Icm42688 { + /// 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> { + 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> { + 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> { + 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 { + 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 { + 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 { + match self.start_configuration(index) { + Ok(()) => Icm42688Event::Initializing, + Err(error) => Icm42688Event::Fault(error), + } + } + + fn start_configuration(&mut self, index: usize) -> Result<(), Error> { + 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> { + 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)) + )); + } +} diff --git a/libs/kaptan-drivers/src/icm42688/sample.rs b/libs/kaptan-drivers/src/icm42688/sample.rs new file mode 100644 index 0000000..7b49ae5 --- /dev/null +++ b/libs/kaptan-drivers/src/icm42688/sample.rs @@ -0,0 +1,57 @@ +//! ICM-42688-P burst-sample 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)), + ) +} + +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. +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). +const fn scale_angular_rate(raw: i16) -> i16 { + ((raw as i32 * 17_453) / 65_500) as i16 +} + +// ICM-42688-P temperature is 132.48 counts/°C with a 25°C offset. +const fn scale_temperature(raw: i16) -> i16 { + ((raw as i32 * 100) / 13_248 + 2_500) as i16 +} + +#[cfg(test)] +mod tests { + use super::decode; + use kaptan_time::Timestamp; + + #[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); + } +} diff --git a/libs/kaptan-drivers/src/icm42688/service.rs b/libs/kaptan-drivers/src/icm42688/service.rs new file mode 100644 index 0000000..857ec3e --- /dev/null +++ b/libs/kaptan-drivers/src/icm42688/service.rs @@ -0,0 +1,82 @@ +//! Timestamped ICM-42688-P acquisition service. + +use super::{Error, Icm42688, Icm42688Event}; +use kaptan_sensor::imu::ImuSnapshot; +use kaptan_spi::{SpiCompletion, SpiDevice}; +use kaptan_time::MonotonicClock; + +/// One observable result from an ICM-42688-P acquisition service. +#[derive(Debug)] +pub enum Icm42688ServiceEvent { + /// No accepted sample or terminal fault was produced. + Idle, + /// IMU configuration completed and the DRDY route may be enabled. + Initialized, + /// A coherent IMU observation is ready for publication or control. + Sample(ImuSnapshot), + /// The ICM-42688-P path entered a terminal fault state. + Fault(Error), +} + +/// Owns ICM-42688-P state, its clock, and sample acceptance. +pub struct Icm42688Service { + clock: Clock, + imu: Icm42688, +} + +impl Icm42688Service { + /// Creates an inactive ICM-42688-P acquisition service. + pub const fn new(clock: Clock, imu: Icm42688) -> Self { + Self { clock, imu } + } +} + +impl Icm42688Service +where + Clock: MonotonicClock, + Bus: SpiDevice, +{ + /// Submits the asynchronous reset transaction. + pub fn begin(&mut self) -> Result<(), Error> { + self.imu.begin_initialization() + } + + /// Advances reset and sensor-start settling states on a periodic tick. + pub fn on_periodic_tick(&mut self) -> Icm42688ServiceEvent { + if self.imu.is_ready() { + return match self.imu.request_sample(self.clock.now()) { + Ok(()) | Err(Error::NotReady) => Icm42688ServiceEvent::Idle, + Err(error) => Icm42688ServiceEvent::Fault(error), + }; + } + let _ = self.clock.now(); + match self.imu.advance_initialization() { + Ok(()) => Icm42688ServiceEvent::Idle, + Err(error) => Icm42688ServiceEvent::Fault(error), + } + } + + /// Handles one coalesced data-ready event. + pub fn on_data_ready(&mut self) -> Icm42688ServiceEvent { + if !self.imu.is_ready() { + return Icm42688ServiceEvent::Idle; + } + match self.imu.request_sample(self.clock.now()) { + Ok(()) | Err(Error::NotReady) => Icm42688ServiceEvent::Idle, + Err(error) => Icm42688ServiceEvent::Fault(error), + } + } + + /// Advances the transaction state from a DMA terminal result. + pub fn on_transfer_complete( + &mut self, + completion: SpiCompletion, + ) -> Icm42688ServiceEvent { + match self.imu.on_transfer_complete(completion) { + Icm42688Event::NotPending | Icm42688Event::Initializing => Icm42688ServiceEvent::Idle, + Icm42688Event::Initialized => Icm42688ServiceEvent::Initialized, + Icm42688Event::Sample(sample) => Icm42688ServiceEvent::Sample(sample), + Icm42688Event::Fault(error) => Icm42688ServiceEvent::Fault(error), + } + } +} diff --git a/libs/kaptan-drivers/src/lib.rs b/libs/kaptan-drivers/src/lib.rs index c1382b2..6a208e4 100644 --- a/libs/kaptan-drivers/src/lib.rs +++ b/libs/kaptan-drivers/src/lib.rs @@ -2,12 +2,16 @@ //! //! 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. +//! layer in a HAL route implementation and its platform composition. #![no_std] +/// TDK InvenSense ICM-42688-P six-axis IMU driver. +pub mod icm42688; /// InvenSense MPU6000 six-axis IMU driver. pub mod mpu6000; /// Garmin LIDAR-Lite v3 I²C rangefinder driver. pub mod lidar_lite_v3; +/// MicoAir MTF-01 Microlink UART range and optical-flow driver. +pub mod mtf01; diff --git a/libs/kaptan-drivers/src/lidar_lite_v3/mod.rs b/libs/kaptan-drivers/src/lidar_lite_v3/mod.rs index fceac00..b00cf1d 100644 --- a/libs/kaptan-drivers/src/lidar_lite_v3/mod.rs +++ b/libs/kaptan-drivers/src/lidar_lite_v3/mod.rs @@ -2,12 +2,14 @@ mod error; mod registers; +pub mod service; use kaptan_i2c::{I2cBus, I2cCompletion}; use kaptan_sensor::range::RangeSnapshot; use kaptan_time::Timestamp; pub use error::{Error, StartupError}; +pub use service::{LidarLiteV3Service, LidarLiteV3ServiceEvent}; use registers::{ ACQ_COMMAND, AUTO_INCREMENT, DEFAULT_ADDRESS, FULL_DELAY_HIGH, @@ -94,7 +96,7 @@ pub enum LidarLiteV3Event { /// The acquisition command was accepted by the device. MeasurementStarted, /// The measurement is still in progress; call [`LidarLiteV3::poll_measurement`] - /// again after a board-selected interval. + /// again after a platform-selected interval. MeasurementBusy, /// The status-read phase is pending after its register selection completed. ReadingStatus, @@ -184,7 +186,7 @@ impl LidarLiteV3 { Ok(()) } - /// Advances the next STOP-separated phase after a board-selected interval. + /// Advances the next STOP-separated phase after a platform-selected interval. /// /// LIDAR-Lite v3 requires a STOP before a new START. The I²C terminal IRQ /// therefore only records phase completion; this periodic path begins the diff --git a/libs/kaptan-drivers/src/lidar_lite_v3/service.rs b/libs/kaptan-drivers/src/lidar_lite_v3/service.rs new file mode 100644 index 0000000..421cb38 --- /dev/null +++ b/libs/kaptan-drivers/src/lidar_lite_v3/service.rs @@ -0,0 +1,180 @@ +//! Timestamped LIDAR-Lite v3 acquisition service. + +use super::{AcquisitionMode, Error, LidarLiteV3, LidarLiteV3Event, StartupError}; +use kaptan_i2c::{I2cBus, I2cCompletion}; +use kaptan_sensor::range::RangeSnapshot; +use kaptan_time::MonotonicClock; + +/// One observable result from a LIDAR-Lite v3 acquisition service. +#[derive(Debug)] +pub enum LidarLiteV3ServiceEvent { + /// No observation or terminal fault was produced. + Idle, + /// One coherent range observation is ready for publication. + Measurement(RangeSnapshot), + /// The LIDAR-Lite v3 path entered a terminal fault state. + Fault(Error), +} + +/// Owns LIDAR-Lite v3 state, its clock, and measurement acceptance. +pub struct LidarLiteV3Service { + clock: Clock, + lidar: LidarLiteV3, + mode: AcquisitionMode, +} + +impl LidarLiteV3Service { + /// Creates an inactive LIDAR-Lite v3 acquisition service. + pub const fn new(clock: Clock, lidar: LidarLiteV3, mode: AcquisitionMode) -> Self { + Self { clock, lidar, mode } + } + + /// Completes the LIDAR-Lite v3 startup interval selected by the board. + pub fn finish_startup(&mut self) -> Result<(), StartupError> { + self.lidar.finish_startup() + } +} + +impl LidarLiteV3Service { + /// Starts a new measurement, or polls one already in progress. + pub fn on_periodic_tick(&mut self) -> LidarLiteV3ServiceEvent { + let now = self.clock.now(); + let result = if self.lidar.is_ready() { + self.lidar.request_measurement(now, self.mode) + } else { + self.lidar.poll_measurement(now) + }; + match result { + Ok(()) | Err(Error::NotReady) => LidarLiteV3ServiceEvent::Idle, + Err(error) => LidarLiteV3ServiceEvent::Fault(error), + } + } + + /// Advances the LIDAR-Lite v3 transaction state from an I²C terminal event. + pub fn on_transaction_complete( + &mut self, + completion: I2cCompletion, + ) -> LidarLiteV3ServiceEvent { + match self.lidar.on_transaction_complete(completion) { + LidarLiteV3Event::Measurement(sample) => LidarLiteV3ServiceEvent::Measurement(sample), + LidarLiteV3Event::Fault(error) => LidarLiteV3ServiceEvent::Fault(error), + _ => LidarLiteV3ServiceEvent::Idle, + } + } + + /// Acknowledges a board-controller interrupt and advances the acquisition. + /// + /// The closure keeps controller-specific interrupt handling at the board + /// boundary while preserving this service as the sole owner of LIDAR state. + pub fn on_controller_interrupt( + &mut self, + acknowledge: F, + ) -> LidarLiteV3ServiceEvent + where + F: FnOnce(&mut Bus) -> I2cCompletion, + { + let completion = self.lidar.acknowledge_with(acknowledge); + self.on_transaction_complete(completion) + } +} + +#[cfg(test)] +mod tests { + use super::{LidarLiteV3Service, LidarLiteV3ServiceEvent}; + use crate::lidar_lite_v3::{AcquisitionMode, LidarLiteV3}; + use kaptan_i2c::{I2cBus, I2cCompletion}; + use kaptan_time::{MonotonicClock, Timestamp}; + + struct Clock; + + impl MonotonicClock for Clock { + fn now(&mut self) -> Timestamp { + Timestamp::from_microseconds(123) + } + } + + struct Bus { + response: [u8; 2], + } + + impl I2cBus for Bus { + type StartError = (); + + fn start_write(&mut self, _: u8, _: &[u8]) -> Result<(), Self::StartError> { + Ok(()) + } + + fn start_read(&mut self, _: u8, _: usize) -> Result<(), Self::StartError> { + Ok(()) + } + + fn complete_transaction(&mut self, completion: I2cCompletion, read: Option<&mut [u8]>) { + if completion == I2cCompletion::Complete + && let Some(read) = read + { + read.copy_from_slice(&self.response[..read.len()]); + } + } + + fn cancel_transaction(&mut self) {} + } + + #[test] + fn produces_a_measurement_after_stop_separated_i2c_transactions() { + let mut service = LidarLiteV3Service::new( + Clock, + LidarLiteV3::new(Bus { response: [0; 2] }), + AcquisitionMode::WithReceiverBiasCorrection, + ); + service.finish_startup().unwrap(); + + assert!(matches!( + service.on_periodic_tick(), + LidarLiteV3ServiceEvent::Idle + )); + assert!(matches!( + service.on_transaction_complete(I2cCompletion::Complete), + LidarLiteV3ServiceEvent::Idle + )); + assert!(matches!( + service.on_periodic_tick(), + LidarLiteV3ServiceEvent::Idle + )); + assert!(matches!( + service.on_controller_interrupt(|bus| { + bus.response = [1 << 5, 0]; + I2cCompletion::Complete + }), + LidarLiteV3ServiceEvent::Idle + )); + assert!(matches!( + service.on_periodic_tick(), + LidarLiteV3ServiceEvent::Idle + )); + assert!(matches!( + service.on_controller_interrupt(|_| I2cCompletion::Complete), + LidarLiteV3ServiceEvent::Idle + )); + assert!(matches!( + service.on_periodic_tick(), + LidarLiteV3ServiceEvent::Idle + )); + assert!(matches!( + service.on_controller_interrupt(|bus| { + bus.response = [0x01, 0x23]; + I2cCompletion::Complete + }), + LidarLiteV3ServiceEvent::Idle + )); + assert!(matches!( + service.on_periodic_tick(), + LidarLiteV3ServiceEvent::Idle + )); + assert!(matches!( + service.on_controller_interrupt(|_| I2cCompletion::Complete), + LidarLiteV3ServiceEvent::Measurement(sample) + if sample.timestamp == Timestamp::from_microseconds(123) + && sample.distance_millimetres == 2_910 + )); + } +} diff --git a/libs/kaptan-drivers/src/mpu6000/mod.rs b/libs/kaptan-drivers/src/mpu6000/mod.rs index eba47bf..f3cc0d4 100644 --- a/libs/kaptan-drivers/src/mpu6000/mod.rs +++ b/libs/kaptan-drivers/src/mpu6000/mod.rs @@ -4,6 +4,7 @@ mod config; mod error; mod registers; mod sample; +pub mod service; use kaptan_sensor::imu::ImuSnapshot; use kaptan_spi::{SpiCompletion, SpiDevice}; @@ -11,6 +12,7 @@ use kaptan_time::Timestamp; pub use config::{AccelerometerRange, DigitalLowPassFilter, GyroscopeRange, Mpu6000Config}; 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, @@ -87,7 +89,7 @@ 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. + /// after the platform-selected reset-settling interval has elapsed. pub fn begin_initialization(&mut self) -> Result<(), Error> { if self.state != State::Dormant { return Err(Error::NotReady); diff --git a/libs/kaptan-drivers/src/mpu6000/service.rs b/libs/kaptan-drivers/src/mpu6000/service.rs new file mode 100644 index 0000000..66b6488 --- /dev/null +++ b/libs/kaptan-drivers/src/mpu6000/service.rs @@ -0,0 +1,157 @@ +//! Timestamped MPU6000 acquisition service. + +use super::{Error, Mpu6000, Mpu6000Event}; +use kaptan_sensor::imu::ImuSnapshot; +use kaptan_spi::{SpiCompletion, SpiDevice}; +use kaptan_time::MonotonicClock; + +/// One observable result from an MPU6000 acquisition service. +#[derive(Debug)] +pub enum Mpu6000ServiceEvent { + /// No accepted sample or terminal fault was produced. + Idle, + /// MPU configuration completed and the DRDY route may be enabled. + Initialized, + /// A coherent IMU observation is ready for publication or control. + Sample(ImuSnapshot), + /// 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. +pub struct Mpu6000Service { + clock: Clock, + imu: Mpu6000, +} + +impl Mpu6000Service { + /// Creates an inactive MPU6000 acquisition service. + pub const fn new(clock: Clock, imu: Mpu6000) -> Self { + Self { clock, imu } + } +} + +impl Mpu6000Service +where + Clock: MonotonicClock, + Bus: SpiDevice, +{ + /// Submits the asynchronous sensor reset transaction. + pub fn begin(&mut self) -> Result<(), Error> { + self.imu.begin_initialization() + } + + /// Advances the reset-settling state on a board periodic event. + pub fn on_periodic_tick(&mut self) -> Mpu6000ServiceEvent { + if self.imu.is_ready() { + return match self.imu.request_sample(self.clock.now()) { + Ok(()) | Err(Error::NotReady) => Mpu6000ServiceEvent::Idle, + Err(error) => Mpu6000ServiceEvent::Fault(error), + }; + } + + let _ = self.clock.now(); + match self.imu.advance_initialization() { + Ok(()) => Mpu6000ServiceEvent::Idle, + Err(error) => Mpu6000ServiceEvent::Fault(error), + } + } + + /// Handles one coalesced data-ready event. + pub fn on_data_ready(&mut self) -> Mpu6000ServiceEvent { + if !self.imu.is_ready() { + return Mpu6000ServiceEvent::Idle; + } + + match self.imu.request_sample(self.clock.now()) { + Ok(()) | Err(Error::NotReady) => Mpu6000ServiceEvent::Idle, + Err(error) => Mpu6000ServiceEvent::Fault(error), + } + } + + /// Advances the MPU6000 transaction state from a DMA terminal result. + pub fn on_transfer_complete( + &mut self, + completion: SpiCompletion, + ) -> Mpu6000ServiceEvent { + match self.imu.on_transfer_complete(completion) { + Mpu6000Event::NotPending | Mpu6000Event::Initializing => Mpu6000ServiceEvent::Idle, + Mpu6000Event::Initialized => Mpu6000ServiceEvent::Initialized, + Mpu6000Event::Sample(sample) => Mpu6000ServiceEvent::Sample(sample), + Mpu6000Event::Fault(error) => Mpu6000ServiceEvent::Fault(error), + } + } +} + +#[cfg(test)] +mod tests { + use super::{Mpu6000Service, Mpu6000ServiceEvent}; + use crate::mpu6000::{Mpu6000, Mpu6000Config}; + use kaptan_spi::{SpiCompletion, SpiDevice}; + use kaptan_time::{MonotonicClock, Timestamp}; + + struct Clock; + + impl MonotonicClock for Clock { + fn now(&mut self) -> Timestamp { + Timestamp::from_microseconds(42) + } + } + + struct Bus { + response: [u8; 15], + } + + 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_initialization() { + let mut response = [0; 15]; + response[1] = 0x68; + let mut service = Mpu6000Service::new( + Clock, + Mpu6000::new(Bus { response }, Mpu6000Config::default()), + ); + + service.begin().unwrap(); + assert!(matches!(service.on_data_ready(), Mpu6000ServiceEvent::Idle)); + assert!(matches!( + service.on_transfer_complete(SpiCompletion::Complete), + Mpu6000ServiceEvent::Idle + )); + assert!(matches!( + service.on_periodic_tick(), + Mpu6000ServiceEvent::Idle + )); + for _ in 0..9 { + assert!(matches!( + service.on_transfer_complete(SpiCompletion::Complete), + Mpu6000ServiceEvent::Idle + )); + } + assert!(matches!( + service.on_transfer_complete(SpiCompletion::Complete), + Mpu6000ServiceEvent::Initialized + )); + assert!(matches!( + service.on_periodic_tick(), + Mpu6000ServiceEvent::Idle + )); + assert!(matches!( + service.on_transfer_complete(SpiCompletion::Complete), + Mpu6000ServiceEvent::Sample(_) + )); + } +} diff --git a/libs/kaptan-drivers/src/mtf01/mod.rs b/libs/kaptan-drivers/src/mtf01/mod.rs new file mode 100644 index 0000000..dedde21 --- /dev/null +++ b/libs/kaptan-drivers/src/mtf01/mod.rs @@ -0,0 +1,471 @@ +//! MicoAir MTF-01 Microlink UART frame decoder. +//! +//! Device documentation belongs beside this module. +//! +//! The MTF-01 continuously transmits fixed-size Microlink range-sensor frames. +//! Feed each received UART byte to [`Mtf01::receive_byte`]. The parser is +//! allocation-free, does not use packed structs, and resynchronizes on a new +//! frame header after malformed input. + +use kaptan_sensor::{optical_flow::OpticalFlowSnapshot, range::RangeSnapshot}; +use kaptan_time::{MonotonicClock, Timestamp}; + +const FRAME_HEADER: u8 = 0xEF; +const RANGE_SENSOR_MESSAGE_ID: u8 = 0x51; +const PAYLOAD_LENGTH: usize = 20; + +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +enum State { + Header, + DeviceId, + SystemId, + MessageId, + Sequence, + Length, + Payload { index: usize }, + Checksum, +} + +/// One complete MTF-01 range and optical-flow report. +/// +/// This preserves the MTF-01's device-specific fields. Use +/// [`Self::range_snapshot`] or [`Self::optical_flow_snapshot`] to obtain a +/// device-independent observation for the rest of the flight stack. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub struct Mtf01Report { + /// Flight-controller monotonic time at frame reception. + pub timestamp: Timestamp, + /// MTF-01 system time, in milliseconds. + pub sensor_time_milliseconds: u32, + /// Measured distance in millimetres. Zero means unavailable. + pub distance_millimetres: u32, + /// MTF-01 range-signal strength, from 0 through 255. + pub distance_strength: u8, + /// MTF-01 distance precision; smaller values indicate higher accuracy. + pub distance_precision: u8, + /// Whether the MTF-01 marked the distance measurement valid. + pub distance_valid: bool, + /// Optical-flow velocity along the MTF-01 X axis, in centimetres/second + /// at one metre above ground. + pub flow_velocity_x_centimetres_per_second_at_one_metre: i16, + /// Optical-flow velocity along the MTF-01 Y axis, in centimetres/second + /// at one metre above ground. + pub flow_velocity_y_centimetres_per_second_at_one_metre: i16, + /// MTF-01 optical-flow quality, from 0 through 255. + pub flow_quality: u8, + /// Whether the MTF-01 marked the optical-flow measurement valid. + pub flow_valid: bool, +} + +impl Mtf01Report { + /// Creates an MTF-01 observation from one validated Microlink frame. + #[allow(clippy::too_many_arguments)] + pub const fn new( + timestamp: Timestamp, + sensor_time_milliseconds: u32, + distance_millimetres: u32, + distance_strength: u8, + distance_precision: u8, + distance_valid: bool, + flow_velocity_x_centimetres_per_second_at_one_metre: i16, + flow_velocity_y_centimetres_per_second_at_one_metre: i16, + flow_quality: u8, + flow_valid: bool, + ) -> Self { + Self { + timestamp, + sensor_time_milliseconds, + distance_millimetres, + distance_strength, + distance_precision, + distance_valid, + flow_velocity_x_centimetres_per_second_at_one_metre, + flow_velocity_y_centimetres_per_second_at_one_metre, + flow_quality, + flow_valid, + } + } + + /// Returns the valid range portion of this report. + pub const fn range_snapshot(self) -> Option { + if self.distance_valid && self.distance_millimetres != 0 { + Some(RangeSnapshot::new( + self.timestamp, + self.distance_millimetres, + )) + } else { + None + } + } + + /// Returns the valid, ground-referenced optical-flow portion of this + /// report. MTF-01 velocities are specified in centimetres per second at + /// one metre; the device documentation scales them by measured height. + pub fn optical_flow_snapshot(self) -> Option { + if !self.distance_valid || self.distance_millimetres == 0 || !self.flow_valid { + return None; + } + + let velocity_x = i64::from(self.flow_velocity_x_centimetres_per_second_at_one_metre) + * i64::from(self.distance_millimetres) + / 100; + let velocity_y = i64::from(self.flow_velocity_y_centimetres_per_second_at_one_metre) + * i64::from(self.distance_millimetres) + / 100; + if velocity_x < i64::from(i32::MIN) + || velocity_x > i64::from(i32::MAX) + || velocity_y < i64::from(i32::MIN) + || velocity_y > i64::from(i32::MAX) + { + return None; + } + + Some(OpticalFlowSnapshot::new( + self.timestamp, + [velocity_x as i32, velocity_y as i32], + self.distance_millimetres, + self.flow_quality, + )) + } +} + +/// MTF-01 Microlink decoder. +/// +/// One decoder is owned by one UART receive path. It accepts the MTF-01's +/// documented range-sensor message ID and 20-byte payload length. Device and +/// system IDs are deliberately accepted as transmitted, matching MicoAir's +/// reference decoder and allowing sensor IDs configured outside factory +/// defaults. +pub struct Mtf01 { + state: State, + checksum: u8, + payload: [u8; PAYLOAD_LENGTH], +} + +impl Mtf01 { + /// Creates a decoder awaiting the `0xEF` Microlink frame header. + pub const fn new() -> Self { + Self { + state: State::Header, + checksum: 0, + payload: [0; PAYLOAD_LENGTH], + } + } + + /// Accepts one UART byte and returns a complete, checksum-validated MTF-01 + /// report when available. + /// + /// `timestamp` should be the controller monotonic time at receipt of this + /// byte. It is used only for a completed frame, so it records the arrival + /// time of the report rather than an arbitrary intermediate byte. + pub fn receive_byte(&mut self, timestamp: Timestamp, byte: u8) -> Option { + match self.state { + State::Header => { + if byte == FRAME_HEADER { + self.checksum = byte; + self.state = State::DeviceId; + } + } + State::DeviceId => { + self.accept(byte, State::SystemId); + } + State::SystemId => { + self.accept(byte, State::MessageId); + } + State::MessageId => { + if byte == RANGE_SENSOR_MESSAGE_ID { + self.accept(byte, State::Sequence); + } else { + self.restart(byte); + } + } + State::Sequence => self.accept(byte, State::Length), + State::Length => { + if usize::from(byte) == PAYLOAD_LENGTH { + self.accept(byte, State::Payload { index: 0 }); + } else { + self.restart(byte); + } + } + State::Payload { index } => { + self.payload[index] = byte; + self.checksum = self.checksum.wrapping_add(byte); + self.state = if index + 1 == PAYLOAD_LENGTH { + State::Checksum + } else { + State::Payload { index: index + 1 } + }; + } + State::Checksum => { + let valid = byte == self.checksum; + // The checksum terminates the frame even when its value is + // also the Microlink header. The reference parser returns to + // its initial state here rather than treating checksum data as + // the start of the next frame. + self.reset(); + if valid { + return Some(self.snapshot(timestamp)); + } + } + } + + None + } + + /// Drops any partial frame and awaits a new header. + pub fn reset(&mut self) { + self.state = State::Header; + self.checksum = 0; + } + + fn accept(&mut self, byte: u8, next: State) { + self.checksum = self.checksum.wrapping_add(byte); + self.state = next; + } + + fn restart(&mut self, byte: u8) { + if byte == FRAME_HEADER { + self.checksum = FRAME_HEADER; + self.state = State::DeviceId; + } else { + self.reset(); + } + } + + fn snapshot(&self, timestamp: Timestamp) -> Mtf01Report { + Mtf01Report::new( + timestamp, + u32::from_le_bytes(self.payload[0..4].try_into().unwrap()), + u32::from_le_bytes(self.payload[4..8].try_into().unwrap()), + self.payload[8], + self.payload[9], + self.payload[10] == 1, + i16::from_le_bytes(self.payload[12..14].try_into().unwrap()), + i16::from_le_bytes(self.payload[14..16].try_into().unwrap()), + self.payload[16], + self.payload[17] == 1, + ) + } +} + +impl Default for Mtf01 { + fn default() -> Self { + Self::new() + } +} + +/// Timestamping MTF-01 UART receive service. +/// +/// Feed this service from the selected UART receive interrupt or DMA consumer. +/// The board retains ownership of pin muxing, buffering, and interrupt +/// acknowledgement. +pub struct Mtf01Service { + clock: Clock, + decoder: Mtf01, +} + +impl Mtf01Service { + /// Creates an MTF-01 receive service awaiting a Microlink frame header. + pub const fn new(clock: Clock) -> Self { + Self { + clock, + decoder: Mtf01::new(), + } + } + + /// Discards a partial frame after a UART transport fault. + pub fn reset(&mut self) { + self.decoder.reset(); + } +} + +impl Mtf01Service { + /// Consumes one UART byte and returns one validated MTF-01 report. + pub fn on_received_byte(&mut self, byte: u8) -> Option { + self.decoder.receive_byte(self.clock.now(), byte) + } +} + +#[cfg(test)] +mod tests { + use super::{Mtf01, Mtf01Report, Mtf01Service}; + use kaptan_time::{MonotonicClock, Timestamp}; + + fn frame(payload: [u8; 20]) -> [u8; 27] { + let mut frame = [0_u8; 27]; + frame[..6].copy_from_slice(&[0xEF, 0x0F, 0x00, 0x51, 0xA5, 20]); + frame[6..26].copy_from_slice(&payload); + frame[26] = frame[..26] + .iter() + .fold(0_u8, |checksum, byte| checksum.wrapping_add(*byte)); + frame + } + + fn sample_frame() -> [u8; 27] { + frame([ + 0x78, 0x56, 0x34, 0x12, // sensor time: 305419896 ms + 0xD2, 0x04, 0x00, 0x00, // distance: 1234 mm + 200, 7, 1, 0, // range metadata and reserved byte + 0x2E, 0xFB, // flow x: -1234 + 0xD2, 0x04, // flow y: 1234 + 250, 1, 0, 0, // flow metadata and reserved u16 + ]) + } + + #[test] + fn decodes_a_valid_little_endian_range_and_flow_report() { + let mut decoder = Mtf01::new(); + let timestamp = Timestamp::from_microseconds(9_876); + let mut report = None; + for byte in sample_frame() { + report = decoder.receive_byte(timestamp, byte).or(report); + } + + assert_eq!( + report, + Some(Mtf01Report::new( + timestamp, + 0x1234_5678, + 1_234, + 200, + 7, + true, + -1_234, + 1_234, + 250, + true, + )) + ); + } + + #[test] + fn rejects_a_bad_checksum_without_publishing_a_report() { + let mut decoder = Mtf01::new(); + let mut corrupt = sample_frame(); + corrupt[26] ^= 0xFF; + + assert!(corrupt.into_iter().all(|byte| { + decoder + .receive_byte(Timestamp::from_microseconds(1), byte) + .is_none() + })); + } + + #[test] + fn treats_a_header_valued_checksum_as_a_checksum() { + let mut decoder = Mtf01::new(); + let mut payload = [0_u8; 20]; + // The fixed header through length sums to 0x08, making 0xE7 produce + // the `0xEF` checksum value that otherwise resembles a new header. + payload[0] = 0xE7; + let checksum_is_header = frame(payload); + assert_eq!(checksum_is_header[26], 0xEF); + + let first_report = checksum_is_header + .into_iter() + .find_map(|byte| decoder.receive_byte(Timestamp::from_microseconds(1), byte)); + assert!(first_report.is_some()); + let report = sample_frame() + .into_iter() + .find_map(|byte| decoder.receive_byte(Timestamp::from_microseconds(2), byte)); + + assert_eq!(report.unwrap().distance_millimetres, 1_234); + } + + #[test] + fn resynchronizes_when_a_header_arrives_during_a_bad_frame() { + let mut decoder = Mtf01::new(); + let valid = sample_frame(); + let mut bytes = [0_u8; 30]; + bytes[..3].copy_from_slice(&[0xEF, 0x0F, 0xEF]); + bytes[3..].copy_from_slice(&valid); + + let mut report = None; + for byte in bytes { + report = decoder + .receive_byte(Timestamp::from_microseconds(7), byte) + .or(report); + } + assert_eq!(report.unwrap().distance_millimetres, 1_234); + } + + #[test] + fn does_not_accept_another_micolink_message_type() { + let mut decoder = Mtf01::new(); + let mut other_message = sample_frame(); + other_message[3] = 0x52; + other_message[26] = other_message[..26] + .iter() + .fold(0_u8, |checksum, byte| checksum.wrapping_add(*byte)); + + assert!(other_message.into_iter().all(|byte| { + decoder + .receive_byte(Timestamp::from_microseconds(1), byte) + .is_none() + })); + } + + #[test] + fn accepts_configured_micolink_device_and_system_ids() { + let mut decoder = Mtf01::new(); + let mut configured_ids = sample_frame(); + configured_ids[1] = 0x42; + configured_ids[2] = 0x99; + configured_ids[26] = configured_ids[..26] + .iter() + .fold(0_u8, |checksum, byte| checksum.wrapping_add(*byte)); + + let report = configured_ids + .into_iter() + .find_map(|byte| decoder.receive_byte(Timestamp::from_microseconds(1), byte)); + assert_eq!(report.unwrap().distance_millimetres, 1_234); + } + + #[test] + fn exposes_only_valid_generic_observations() { + let mut report = Mtf01Report::new( + Timestamp::from_microseconds(123), + 7, + 2_000, + 8, + 9, + true, + 50, + -25, + 12, + true, + ); + assert_eq!(report.range_snapshot().unwrap().distance_millimetres, 2_000); + assert_eq!( + report + .optical_flow_snapshot() + .unwrap() + .velocity_millimetres_per_second, + [1_000, -500] + ); + + report.distance_valid = false; + assert_eq!(report.range_snapshot(), None); + assert_eq!(report.optical_flow_snapshot(), None); + } + + struct Clock(u64); + + impl MonotonicClock for Clock { + fn now(&mut self) -> Timestamp { + self.0 += 1; + Timestamp::from_microseconds(self.0) + } + } + + #[test] + fn timestamps_a_report_with_its_last_received_byte() { + let mut service = Mtf01Service::new(Clock(100)); + let report = sample_frame() + .into_iter() + .find_map(|byte| service.on_received_byte(byte)) + .unwrap(); + + assert_eq!(report.timestamp, Timestamp::from_microseconds(127)); + } +} diff --git a/libs/kaptan-drivers/src/mtf01/mtf01-decoding.pdf b/libs/kaptan-drivers/src/mtf01/mtf01-decoding.pdf new file mode 100644 index 0000000..9716e7c Binary files /dev/null and b/libs/kaptan-drivers/src/mtf01/mtf01-decoding.pdf differ