libs/kaptan-runtime: add range acquisition service

libs/kaptan-runtime/src/lib.rs

@@ -11,6 +11,10 @@
 pub mod imu;
 /// Lock-free worker handoff primitives.
 pub mod mailbox;
+/// Single-owner LIDAR-Lite range acquisition service.
+pub mod range;
 
 pub use imu::{ImuEvent, ImuService};
 pub use mailbox::LatestImuMailbox;
+pub use mailbox::LatestRangeMailbox;
+pub use range::{RangeEvent, RangeService};

libs/kaptan-runtime/src/range.rs

@@ -0,0 +1,165 @@
+//! Single-owner LIDAR-Lite range acquisition service.
+
+use kaptan_drivers::lidar_lite_v3::{
+    AcquisitionMode, Error as LidarError, LidarLiteV3, LidarLiteV3Event, StartupError,
+};
+use kaptan_i2c::{I2cBus, I2cCompletion};
+use kaptan_sensor::range::RangeSnapshot;
+use kaptan_time::MonotonicClock;
+
+/// One observable result from the range worker.
+#[derive(Debug)]
+pub enum RangeEvent<BusError> {
+    /// No observation or terminal fault was produced.
+    Idle,
+    /// One coherent range observation is ready for publication.
+    Measurement(RangeSnapshot),
+    /// The selected sensor path entered a terminal fault state.
+    Fault(LidarError<BusError>),
+}
+
+/// Owns LIDAR-Lite state, its clock, and measurement acceptance.
+pub struct RangeService<Clock, Bus> {
+    clock: Clock,
+    lidar: LidarLiteV3<Bus>,
+    mode: AcquisitionMode,
+}
+
+impl<Clock, Bus> RangeService<Clock, Bus> {
+    /// Creates an inactive range acquisition service.
+    pub const fn new(clock: Clock, lidar: LidarLiteV3<Bus>, mode: AcquisitionMode) -> Self {
+        Self { clock, lidar, mode }
+    }
+
+    /// Completes the LIDAR-Lite startup interval selected by the board.
+    pub fn finish_startup(&mut self) -> Result<(), StartupError> {
+        self.lidar.finish_startup()
+    }
+}
+
+impl<Clock: MonotonicClock, Bus: I2cBus> RangeService<Clock, Bus> {
+    /// Starts a new measurement, or polls one already in progress.
+    pub fn on_periodic_tick(&mut self) -> RangeEvent<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(LidarError::NotReady) => RangeEvent::Idle,
+            Err(error) => RangeEvent::Fault(error),
+        }
+    }
+
+    /// Advances the LIDAR-Lite transaction state from an I²C terminal event.
+    pub fn on_transaction_complete(
+        &mut self,
+        completion: I2cCompletion,
+    ) -> RangeEvent<Bus::StartError> {
+        match self.lidar.on_transaction_complete(completion) {
+            LidarLiteV3Event::Measurement(sample) => RangeEvent::Measurement(sample),
+            LidarLiteV3Event::Fault(error) => RangeEvent::Fault(error),
+            _ => RangeEvent::Idle,
+        }
+    }
+
+    /// Acknowledges a board-controller interrupt and advances the acquisition.
+    ///
+    /// The closure keeps the controller-specific interrupt handling at the
+    /// board boundary while preserving this service as the sole owner of the
+    /// LIDAR state machine.
+    pub fn on_controller_interrupt<F>(&mut self, acknowledge: F) -> RangeEvent<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::{RangeEvent, RangeService};
+    use kaptan_drivers::lidar_lite_v3::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 = RangeService::new(
+            Clock,
+            LidarLiteV3::new(Bus { response: [0; 2] }),
+            kaptan_drivers::lidar_lite_v3::AcquisitionMode::WithReceiverBiasCorrection,
+        );
+        service.finish_startup().unwrap();
+
+        assert!(matches!(service.on_periodic_tick(), RangeEvent::Idle));
+        assert!(matches!(
+            service.on_transaction_complete(I2cCompletion::Complete),
+            RangeEvent::Idle
+        ));
+        assert!(matches!(service.on_periodic_tick(), RangeEvent::Idle));
+        assert!(matches!(
+            service.on_controller_interrupt(|bus| {
+                bus.response = [1 << 5, 0];
+                I2cCompletion::Complete
+            }),
+            RangeEvent::Idle
+        ));
+        assert!(matches!(service.on_periodic_tick(), RangeEvent::Idle));
+        assert!(matches!(
+            service.on_controller_interrupt(|_| I2cCompletion::Complete),
+            RangeEvent::Idle
+        ));
+        assert!(matches!(service.on_periodic_tick(), RangeEvent::Idle));
+        assert!(matches!(
+            service.on_controller_interrupt(|bus| {
+                bus.response = [0x01, 0x23];
+                I2cCompletion::Complete
+            }),
+            RangeEvent::Idle
+        ));
+        assert!(matches!(service.on_periodic_tick(), RangeEvent::Idle));
+        assert!(matches!(
+            service.on_controller_interrupt(|_| I2cCompletion::Complete),
+            RangeEvent::Measurement(sample)
+                if sample.timestamp == Timestamp::from_microseconds(123)
+                    && sample.distance_millimetres == 2_910
+        ));
+    }
+}