libs/kaptan-runtime: add single-owner IMU service

libs/kaptan-runtime/src/imu.rs

@@ -0,0 +1,137 @@
+//! Single-owner MPU acquisition service.
+
+use kaptan_drivers::mpu6000::{Error as Mpu6000Error, Mpu6000, Mpu6000Event};
+use kaptan_sensor::ImuSnapshot;
+use kaptan_spi::{SpiCompletion, SpiDevice};
+use kaptan_time::MonotonicClock;
+
+/// One observable result from the IMU worker.
+#[derive(Debug)]
+pub enum ImuEvent<BusError> {
+    /// 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 telemetry or control.
+    Sample(ImuSnapshot),
+    /// The selected sensor path entered a terminal fault state.
+    Fault(Mpu6000Error<BusError>),
+}
+
+/// Owns MPU state, its clock, and sample acceptance.
+///
+/// Exactly one worker invokes this service. Hardware ISRs only report deferred
+/// DRDY, timer, and SPI-DMA terminal events to that worker.
+pub struct ImuService<Clock, Bus> {
+    clock: Clock,
+    imu: Mpu6000<Bus>,
+}
+
+impl<Clock, Bus> ImuService<Clock, Bus> {
+    /// Creates an inactive acquisition service.
+    pub const fn new(clock: Clock, imu: Mpu6000<Bus>) -> Self {
+        Self { clock, imu }
+    }
+}
+
+impl<Clock, Bus> ImuService<Clock, Bus>
+where
+    Clock: MonotonicClock,
+    Bus: SpiDevice,
+{
+    /// Submits the asynchronous sensor reset transaction.
+    pub fn begin(&mut self) -> Result<(), Mpu6000Error<Bus::StartError>> {
+        self.imu.begin_initialization()
+    }
+
+    /// Advances the reset-settling state on a board periodic event.
+    pub fn on_periodic_tick(&mut self) -> ImuEvent<Bus::StartError> {
+        let _ = self.clock.now();
+        match self.imu.advance_initialization() {
+            Ok(()) => ImuEvent::Idle,
+            Err(error) => ImuEvent::Fault(error),
+        }
+    }
+
+    /// Handles one coalesced data-ready event.
+    pub fn on_data_ready(&mut self) -> ImuEvent<Bus::StartError> {
+        if !self.imu.is_ready() {
+            return ImuEvent::Idle;
+        }
+
+        match self.imu.request_sample(self.clock.now()) {
+            Ok(()) | Err(Mpu6000Error::NotReady) => ImuEvent::Idle,
+            Err(error) => ImuEvent::Fault(error),
+        }
+    }
+
+    /// Advances the MPU transaction state from a DMA terminal result.
+    pub fn on_transfer_complete(&mut self, completion: SpiCompletion) -> ImuEvent<Bus::StartError> {
+        match self.imu.on_transfer_complete(completion) {
+            Mpu6000Event::NotPending | Mpu6000Event::Initializing => ImuEvent::Idle,
+            Mpu6000Event::Initialized => ImuEvent::Initialized,
+            Mpu6000Event::Sample(sample) => ImuEvent::Sample(sample),
+            Mpu6000Event::Fault(error) => ImuEvent::Fault(error),
+        }
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::{ImuEvent, ImuService};
+    use kaptan_drivers::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 = ImuService::new(
+            Clock,
+            Mpu6000::new(Bus { response }, Mpu6000Config::default()),
+        );
+
+        service.begin().unwrap();
+        assert!(matches!(service.on_data_ready(), ImuEvent::Idle));
+        assert!(matches!(
+            service.on_transfer_complete(SpiCompletion::Complete),
+            ImuEvent::Idle
+        ));
+        assert!(matches!(service.on_periodic_tick(), ImuEvent::Idle));
+        for _ in 0..9 {
+            assert!(matches!(
+                service.on_transfer_complete(SpiCompletion::Complete),
+                ImuEvent::Idle
+            ));
+        }
+        assert!(matches!(
+            service.on_transfer_complete(SpiCompletion::Complete),
+            ImuEvent::Initialized
+        ));
+        assert!(matches!(service.on_data_ready(), ImuEvent::Idle));
+    }
+}