libs/kaptan-drivers: add F722 sensor services

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<BusError> {
+    /// 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<BusError>),
+}
+
+/// Owns ICM-42688-P state, its clock, and sample acceptance.
+pub struct Icm42688Service<Clock, Bus> {
+    clock: Clock,
+    imu: Icm42688<Bus>,
+}
+
+impl<Clock, Bus> Icm42688Service<Clock, Bus> {
+    /// Creates an inactive ICM-42688-P acquisition service.
+    pub const fn new(clock: Clock, imu: Icm42688<Bus>) -> Self {
+        Self { clock, imu }
+    }
+}
+
+impl<Clock, Bus> Icm42688Service<Clock, Bus>
+where
+    Clock: MonotonicClock,
+    Bus: SpiDevice,
+{
+    /// Submits the asynchronous reset transaction.
+    pub fn begin(&mut self) -> Result<(), Error<Bus::StartError>> {
+        self.imu.begin_initialization()
+    }
+
+    /// Advances reset and sensor-start settling states on a periodic tick.
+    pub fn on_periodic_tick(&mut self) -> Icm42688ServiceEvent<Bus::StartError> {
+        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<Bus::StartError> {
+        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<Bus::StartError> {
+        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),
+        }
+    }
+}