libs/kaptan-drivers: add F722 sensor services

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<BusError> {
+    /// 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<BusError>),
+}
+
+/// Owns LIDAR-Lite v3 state, its clock, and measurement acceptance.
+pub struct LidarLiteV3Service<Clock, Bus> {
+    clock: Clock,
+    lidar: LidarLiteV3<Bus>,
+    mode: AcquisitionMode,
+}
+
+impl<Clock, Bus> LidarLiteV3Service<Clock, Bus> {
+    /// Creates an inactive LIDAR-Lite v3 acquisition service.
+    pub const fn new(clock: Clock, lidar: LidarLiteV3<Bus>, 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<Clock: MonotonicClock, Bus: I2cBus> LidarLiteV3Service<Clock, Bus> {
+    /// Starts a new measurement, or polls one already in progress.
+    pub fn on_periodic_tick(&mut self) -> LidarLiteV3ServiceEvent<Bus::StartError> {
+        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<Bus::StartError> {
+        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<F>(
+        &mut self,
+        acknowledge: F,
+    ) -> LidarLiteV3ServiceEvent<Bus::StartError>
+    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
+        ));
+    }
+}