From ba668504035fab05f0b68e660230624e5b6d5d11 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Onur=20=C3=96zkan?= Date: Sun, 26 Jul 2026 18:03:31 +0300 Subject: [PATCH] libs/kaptan-runtime: add range acquisition service --- libs/kaptan-runtime/src/lib.rs | 4 + libs/kaptan-runtime/src/range.rs | 165 +++++++++++++++++++++++++++++++ 2 files changed, 169 insertions(+) create mode 100644 libs/kaptan-runtime/src/range.rs diff --git a/libs/kaptan-runtime/src/lib.rs b/libs/kaptan-runtime/src/lib.rs index f107a6d..6c70b17 100644 --- a/libs/kaptan-runtime/src/lib.rs +++ b/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}; diff --git a/libs/kaptan-runtime/src/range.rs b/libs/kaptan-runtime/src/range.rs new file mode 100644 index 0000000..137f1dd --- /dev/null +++ b/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 { + /// 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), +} + +/// Owns LIDAR-Lite state, its clock, and measurement acceptance. +pub struct RangeService { + clock: Clock, + lidar: LidarLiteV3, + mode: AcquisitionMode, +} + +impl RangeService { + /// Creates an inactive range acquisition service. + pub const fn new(clock: Clock, lidar: LidarLiteV3, 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 RangeService { + /// Starts a new measurement, or polls one already in progress. + pub fn on_periodic_tick(&mut self) -> RangeEvent { + 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 { + 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(&mut self, acknowledge: F) -> RangeEvent + 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 + )); + } +}