From 4065aed96182fcb3db28a3a6d4353e7468a07f46 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Onur=20=C3=96zkan?= Date: Sun, 26 Jul 2026 18:03:22 +0300 Subject: [PATCH] libs/kaptan-drivers: add LIDAR-Lite v3 state machine --- libs/kaptan-drivers/src/lib.rs | 3 + .../kaptan-drivers/src/lidar_lite_v3/error.rs | 29 + libs/kaptan-drivers/src/lidar_lite_v3/mod.rs | 779 ++++++++++++++++++ .../src/lidar_lite_v3/registers.rs | 14 + 4 files changed, 825 insertions(+) create mode 100644 libs/kaptan-drivers/src/lidar_lite_v3/error.rs create mode 100644 libs/kaptan-drivers/src/lidar_lite_v3/mod.rs create mode 100644 libs/kaptan-drivers/src/lidar_lite_v3/registers.rs diff --git a/libs/kaptan-drivers/src/lib.rs b/libs/kaptan-drivers/src/lib.rs index 2ac0658..c1382b2 100644 --- a/libs/kaptan-drivers/src/lib.rs +++ b/libs/kaptan-drivers/src/lib.rs @@ -8,3 +8,6 @@ /// InvenSense MPU6000 six-axis IMU driver. pub mod mpu6000; + +/// Garmin LIDAR-Lite v3 I²C rangefinder driver. +pub mod lidar_lite_v3; diff --git a/libs/kaptan-drivers/src/lidar_lite_v3/error.rs b/libs/kaptan-drivers/src/lidar_lite_v3/error.rs new file mode 100644 index 0000000..46eb9b6 --- /dev/null +++ b/libs/kaptan-drivers/src/lidar_lite_v3/error.rs @@ -0,0 +1,29 @@ +//! LIDAR-Lite v3 operation errors. + +/// LIDAR-Lite v3 operation failure. +#[derive(Debug, Eq, PartialEq)] +pub enum Error { + /// The selected I²C route rejected a transaction submission. + Start(BusError), + /// An acquisition request arrived while another measurement was active. + NotReady, + /// The selected I²C route reported a terminal transaction fault. + TransferFault, + /// The device reported a process error for the completed measurement. + ProcessError, + /// The device reported that its reference or receiver bias is unhealthy. + Unhealthy, + /// The device reported that no usable signal peak was detected. + InvalidSignal, + /// A measurement remained busy beyond the driver's bounded acquisition interval. + MeasurementTimeout, + /// The device reported a documented no-signal sentinel distance of zero or one centimetre. + NoSignal, +} + +/// LIDAR-Lite v3 startup lifecycle failure. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub enum StartupError { + /// Startup completion was reported when the driver was not settling. + NotSettling, +} diff --git a/libs/kaptan-drivers/src/lidar_lite_v3/mod.rs b/libs/kaptan-drivers/src/lidar_lite_v3/mod.rs new file mode 100644 index 0000000..fceac00 --- /dev/null +++ b/libs/kaptan-drivers/src/lidar_lite_v3/mod.rs @@ -0,0 +1,779 @@ +//! Garmin LIDAR-Lite v3 register protocol driver. + +mod error; +mod registers; + +use kaptan_i2c::{I2cBus, I2cCompletion}; +use kaptan_sensor::range::RangeSnapshot; +use kaptan_time::Timestamp; + +pub use error::{Error, StartupError}; + +use registers::{ + ACQ_COMMAND, AUTO_INCREMENT, DEFAULT_ADDRESS, FULL_DELAY_HIGH, + MAX_NO_SIGNAL_DISTANCE_CENTIMETRES, MEASURE_WITH_RECEIVER_BIAS, MEASURE_WITHOUT_RECEIVER_BIAS, + STATUS, STATUS_BUSY, STATUS_HEALTHY, STATUS_INVALID_SIGNAL, STATUS_PROCESS_ERROR, +}; + +const COMMAND_LENGTH: usize = 2; +const STATUS_LENGTH: usize = 1; +const DISTANCE_LENGTH: usize = 2; +/// An acquisition with receiver-bias correction may take up to 135 ms. +/// +/// The extra margin preserves liveness if the optical engine remains busy +/// while still accepting a documented slow acquisition. +const MEASUREMENT_TIMEOUT_MICROSECONDS: u64 = 200_000; + +/// Receiver-bias correction selection for one acquisition. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub enum AcquisitionMode { + /// Perform receiver-bias correction before measuring. + WithReceiverBiasCorrection, + /// Omit receiver-bias correction for a faster measurement. + /// + /// The manufacturer recommends a corrected measurement periodically when + /// using this mode repeatedly. + WithoutReceiverBiasCorrection, +} + +impl AcquisitionMode { + const fn command(self) -> u8 { + match self { + Self::WithReceiverBiasCorrection => MEASURE_WITH_RECEIVER_BIAS, + Self::WithoutReceiverBiasCorrection => MEASURE_WITHOUT_RECEIVER_BIAS, + } + } +} + +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +enum State { + Settling, + Idle, + Starting { timestamp: Timestamp }, + Waiting { timestamp: Timestamp }, + WritingStatusRegister { timestamp: Timestamp }, + StatusReadPending { timestamp: Timestamp }, + ReadingStatus { timestamp: Timestamp }, + DistanceRegisterPending { timestamp: Timestamp }, + WritingDistanceRegister { timestamp: Timestamp }, + DistanceReadPending { timestamp: Timestamp }, + ReadingDistance { timestamp: Timestamp }, +} + +/// A LIDAR-Lite v3 7-bit I²C address. +/// +/// The device accepts its default address, `98`, and configurable addresses +/// with a clear least-significant bit, as specified by its address register. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub struct LidarLiteV3Address(u8); + +impl LidarLiteV3Address { + /// The device's factory-default 7-bit I²C address. + pub const DEFAULT: Self = Self(DEFAULT_ADDRESS); + + /// Validates a configured LIDAR-Lite v3 7-bit I²C address. + pub const fn new(address: u8) -> Option { + if address <= 127 && address & 1 == 0 { + Some(Self(address)) + } else { + None + } + } + + /// Returns the address in the 7-bit representation required by [`I2cBus`]. + pub const fn as_7bit(self) -> u8 { + self.0 + } +} + +/// Observable outcome of one I²C terminal event. +#[derive(Debug, Eq, PartialEq)] +pub enum LidarLiteV3Event { + /// The selected I²C interrupt had no terminal transaction event. + NotPending, + /// The acquisition command was accepted by the device. + MeasurementStarted, + /// The measurement is still in progress; call [`LidarLiteV3::poll_measurement`] + /// again after a board-selected interval. + MeasurementBusy, + /// The status-read phase is pending after its register selection completed. + ReadingStatus, + /// The distance-register selection phase is pending after the device became ready. + SelectingDistanceRegister, + /// The final distance-read phase is pending after its register selection completed. + ReadingMeasurement, + /// One valid range observation completed. + Measurement(RangeSnapshot), + /// Acquisition or transport failed. + Fault(Error), +} + +/// LIDAR-Lite v3 driver using its I²C interface. +/// +/// This state machine neither delays nor polls hardware. The board calls +/// [`Self::poll_measurement`] periodically to start each STOP-separated phase +/// and reports I²C completion from its controller interrupt path. +pub struct LidarLiteV3 { + bus: Bus, + address: LidarLiteV3Address, + state: State, + command: [u8; COMMAND_LENGTH], + register: [u8; STATUS_LENGTH], + receive: [u8; DISTANCE_LENGTH], +} + +impl LidarLiteV3 { + /// Creates a driver for a device using its default 7-bit address, `98`. + /// + /// Call [`Self::finish_startup`] only after the board has waited the + /// device's required 22 ms power-on settling interval. + pub const fn new(bus: Bus) -> Self { + Self::with_address(bus, LidarLiteV3Address::DEFAULT) + } + + /// Creates a driver for a device with a previously configured address. + /// + /// Call [`Self::finish_startup`] only after the board has waited the + /// device's required 22 ms power-on settling interval. + pub const fn with_address(bus: Bus, address: LidarLiteV3Address) -> Self { + Self { + bus, + address, + state: State::Settling, + command: [0; COMMAND_LENGTH], + register: [0; STATUS_LENGTH], + receive: [0; DISTANCE_LENGTH], + } + } + + /// Marks the device ready after the board has observed its 22 ms power-on + /// settling interval. + pub fn finish_startup(&mut self) -> Result<(), StartupError> { + if self.state != State::Settling { + return Err(StartupError::NotSettling); + } + self.state = State::Idle; + Ok(()) + } + + /// Acknowledges a controller interrupt against the owned bus. + /// + /// The callback must only acknowledge the current controller interrupt; + /// starting another transaction would violate this driver's phase state. + pub fn acknowledge_with(&mut self, callback: impl FnOnce(&mut Bus) -> T) -> T { + callback(&mut self.bus) + } +} + +impl LidarLiteV3 { + /// Starts one distance acquisition without waiting for I²C or the device. + pub fn request_measurement( + &mut self, + timestamp: Timestamp, + mode: AcquisitionMode, + ) -> Result<(), Error> { + if self.state != State::Idle { + return Err(Error::NotReady); + } + + self.command = [ACQ_COMMAND, mode.command()]; + self.bus + .start_write(self.address.as_7bit(), &self.command) + .map_err(Error::Start)?; + self.state = State::Starting { timestamp }; + Ok(()) + } + + /// Advances the next STOP-separated phase after a board-selected interval. + /// + /// LIDAR-Lite v3 requires a STOP before a new START. The I²C terminal IRQ + /// therefore only records phase completion; this periodic path begins the + /// following transaction after the bus has been released. + pub fn poll_measurement(&mut self, now: Timestamp) -> Result<(), Error> { + let timestamp = match self.state { + State::Waiting { timestamp } + | State::StatusReadPending { timestamp } + | State::DistanceRegisterPending { timestamp } + | State::DistanceReadPending { timestamp } => timestamp, + State::Settling | State::Idle => return Err(Error::NotReady), + _ => { + if self.measurement_timed_out(now) { + self.bus.cancel_transaction(); + self.state = State::Idle; + return Err(Error::MeasurementTimeout); + } + return Err(Error::NotReady); + } + }; + if self.measurement_timed_out(now) { + self.bus.cancel_transaction(); + self.state = State::Idle; + return Err(Error::MeasurementTimeout); + } + match self.state { + State::Waiting { .. } => self.start_status_read(timestamp), + State::StatusReadPending { .. } => self.start_read_status(timestamp), + State::DistanceRegisterPending { .. } => self.start_distance_read(timestamp), + State::DistanceReadPending { .. } => self.start_read_distance(timestamp), + _ => unreachable!(), + } + } + + /// Advances the state machine from an already-acknowledged I²C terminal event. + pub fn on_transaction_complete( + &mut self, + completion: I2cCompletion, + ) -> LidarLiteV3Event { + if completion == I2cCompletion::NotPending || !self.has_active_transaction() { + return LidarLiteV3Event::NotPending; + } + + match completion { + I2cCompletion::TransferFault => { + self.bus.complete_transaction(completion, None); + self.state = State::Idle; + LidarLiteV3Event::Fault(Error::TransferFault) + } + I2cCompletion::Complete => { + let read = match self.state { + State::ReadingStatus { .. } => Some(&mut self.receive[..STATUS_LENGTH]), + State::ReadingDistance { .. } => Some(&mut self.receive[..DISTANCE_LENGTH]), + _ => None, + }; + self.bus.complete_transaction(completion, read); + self.advance_after_completion() + } + I2cCompletion::NotPending => unreachable!(), + } + } + + /// Returns whether a new acquisition can be requested. + pub fn is_ready(&self) -> bool { + self.state == State::Idle + } + + fn has_active_transaction(&self) -> bool { + matches!( + self.state, + State::Starting { .. } + | State::WritingStatusRegister { .. } + | State::ReadingStatus { .. } + | State::WritingDistanceRegister { .. } + | State::ReadingDistance { .. } + ) + } + + fn measurement_timed_out(&self, now: Timestamp) -> bool { + let timestamp = match self.state { + State::Starting { timestamp } + | State::Waiting { timestamp } + | State::WritingStatusRegister { timestamp } + | State::StatusReadPending { timestamp } + | State::ReadingStatus { timestamp } + | State::DistanceRegisterPending { timestamp } + | State::WritingDistanceRegister { timestamp } + | State::DistanceReadPending { timestamp } + | State::ReadingDistance { timestamp } => timestamp, + State::Settling | State::Idle => return false, + }; + now.as_microseconds() + .saturating_sub(timestamp.as_microseconds()) + >= MEASUREMENT_TIMEOUT_MICROSECONDS + } + + fn advance_after_completion(&mut self) -> LidarLiteV3Event { + match self.state { + State::Settling => LidarLiteV3Event::NotPending, + State::Idle => LidarLiteV3Event::NotPending, + State::Starting { timestamp } => { + self.state = State::Waiting { timestamp }; + LidarLiteV3Event::MeasurementStarted + } + State::Waiting { .. } => LidarLiteV3Event::NotPending, + State::WritingStatusRegister { timestamp } => { + self.state = State::StatusReadPending { timestamp }; + LidarLiteV3Event::ReadingStatus + } + State::StatusReadPending { .. } => LidarLiteV3Event::NotPending, + State::ReadingStatus { timestamp } => { + if self.receive[0] & STATUS_BUSY != 0 { + self.state = State::Waiting { timestamp }; + LidarLiteV3Event::MeasurementBusy + } else if self.receive[0] & STATUS_PROCESS_ERROR != 0 { + self.state = State::Idle; + LidarLiteV3Event::Fault(Error::ProcessError) + } else if self.receive[0] & STATUS_INVALID_SIGNAL != 0 { + self.state = State::Idle; + LidarLiteV3Event::Fault(Error::InvalidSignal) + } else if self.receive[0] & STATUS_HEALTHY == 0 { + self.state = State::Idle; + LidarLiteV3Event::Fault(Error::Unhealthy) + } else { + self.state = State::DistanceRegisterPending { timestamp }; + LidarLiteV3Event::SelectingDistanceRegister + } + } + State::DistanceRegisterPending { .. } => LidarLiteV3Event::NotPending, + State::WritingDistanceRegister { timestamp } => { + self.state = State::DistanceReadPending { timestamp }; + LidarLiteV3Event::ReadingMeasurement + } + State::DistanceReadPending { .. } => LidarLiteV3Event::NotPending, + State::ReadingDistance { timestamp } => { + let distance_centimetres = u16::from_be_bytes(self.receive); + self.state = State::Idle; + if distance_centimetres <= MAX_NO_SIGNAL_DISTANCE_CENTIMETRES { + LidarLiteV3Event::Fault(Error::NoSignal) + } else { + LidarLiteV3Event::Measurement(RangeSnapshot::new( + timestamp, + u32::from(distance_centimetres) * 10, + )) + } + } + } + } + + fn start_status_read(&mut self, timestamp: Timestamp) -> Result<(), Error> { + self.register[0] = STATUS; + self.receive = [0; DISTANCE_LENGTH]; + self.bus + .start_write(self.address.as_7bit(), &self.register) + .map_err(Error::Start)?; + self.state = State::WritingStatusRegister { timestamp }; + Ok(()) + } + + fn start_read_status(&mut self, timestamp: Timestamp) -> Result<(), Error> { + self.bus + .start_read(self.address.as_7bit(), STATUS_LENGTH) + .map_err(Error::Start)?; + self.state = State::ReadingStatus { timestamp }; + Ok(()) + } + + fn start_distance_read(&mut self, timestamp: Timestamp) -> Result<(), Error> { + self.register[0] = FULL_DELAY_HIGH | AUTO_INCREMENT; + self.receive = [0; DISTANCE_LENGTH]; + self.bus + .start_write(self.address.as_7bit(), &self.register) + .map_err(Error::Start)?; + self.state = State::WritingDistanceRegister { timestamp }; + Ok(()) + } + + fn start_read_distance(&mut self, timestamp: Timestamp) -> Result<(), Error> { + self.bus + .start_read(self.address.as_7bit(), DISTANCE_LENGTH) + .map_err(Error::Start)?; + self.state = State::ReadingDistance { timestamp }; + Ok(()) + } +} + +#[cfg(test)] +mod tests { + use super::registers::{STATUS_HEALTHY, STATUS_INVALID_SIGNAL, STATUS_PROCESS_ERROR}; + use super::{ + AcquisitionMode, Error, LidarLiteV3, LidarLiteV3Address, LidarLiteV3Event, + MEASUREMENT_TIMEOUT_MICROSECONDS, StartupError, + }; + use kaptan_i2c::{I2cBus, I2cCompletion}; + use kaptan_sensor::range::RangeSnapshot; + use kaptan_time::Timestamp; + + #[derive(Debug, Eq, PartialEq)] + enum FakeError { + Start, + } + + #[derive(Clone, Copy, Debug, Eq, PartialEq)] + enum Transaction { + None, + Write { + address: u8, + bytes: [u8; 2], + length: usize, + }, + Read { + address: u8, + length: usize, + }, + } + + struct FakeBus { + transaction: Transaction, + response: [u8; 2], + fail_start: bool, + completions: usize, + cancellations: usize, + } + + impl FakeBus { + fn ready() -> Self { + Self { + transaction: Transaction::None, + response: [0; 2], + fail_start: false, + completions: 0, + cancellations: 0, + } + } + } + + fn initialized() -> LidarLiteV3 { + let mut driver = LidarLiteV3::new(FakeBus::ready()); + driver.finish_startup().unwrap(); + driver + } + + impl I2cBus for FakeBus { + type StartError = FakeError; + + fn start_write(&mut self, address: u8, write: &[u8]) -> Result<(), Self::StartError> { + if self.fail_start { + return Err(FakeError::Start); + } + let mut bytes = [0; 2]; + bytes[..write.len()].copy_from_slice(write); + self.transaction = Transaction::Write { + address, + bytes, + length: write.len(), + }; + Ok(()) + } + + fn start_read(&mut self, address: u8, length: usize) -> Result<(), Self::StartError> { + if self.fail_start { + return Err(FakeError::Start); + } + self.transaction = Transaction::Read { address, length }; + 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()]); + } + self.completions += 1; + } + + fn cancel_transaction(&mut self) { + self.cancellations += 1; + } + } + + #[test] + fn waits_for_startup_and_rejects_invalid_addresses() { + let mut driver = LidarLiteV3::new(FakeBus::ready()); + assert!(!driver.is_ready()); + assert_eq!( + driver.on_transaction_complete(I2cCompletion::NotPending), + LidarLiteV3Event::NotPending + ); + assert_eq!(driver.bus.completions, 0); + assert_eq!( + driver.on_transaction_complete(I2cCompletion::TransferFault), + LidarLiteV3Event::NotPending + ); + assert_eq!(driver.bus.completions, 0); + assert!(!driver.is_ready()); + assert!(matches!( + driver.request_measurement( + Timestamp::from_microseconds(0), + AcquisitionMode::WithReceiverBiasCorrection, + ), + Err(Error::NotReady) + )); + driver.finish_startup().unwrap(); + assert!(driver.is_ready()); + assert_eq!(driver.finish_startup(), Err(StartupError::NotSettling)); + assert_eq!(LidarLiteV3Address::new(98).unwrap().as_7bit(), 98); + assert_eq!(LidarLiteV3Address::new(196), None); + assert_eq!(LidarLiteV3Address::new(99), None); + } + + #[test] + fn measures_after_stop_separated_status_and_distance_reads() { + let mut driver = initialized(); + let timestamp = Timestamp::from_microseconds(42); + driver + .request_measurement(timestamp, AcquisitionMode::WithReceiverBiasCorrection) + .unwrap(); + assert_eq!( + driver.bus.transaction, + Transaction::Write { + address: LidarLiteV3Address::DEFAULT.as_7bit(), + bytes: [0, 4], + length: 2 + } + ); + assert_eq!( + driver.on_transaction_complete(I2cCompletion::Complete), + LidarLiteV3Event::MeasurementStarted + ); + driver.poll_measurement(timestamp).unwrap(); + assert_eq!( + driver.bus.transaction, + Transaction::Write { + address: LidarLiteV3Address::DEFAULT.as_7bit(), + bytes: [1, 0], + length: 1 + } + ); + driver.bus.response = [STATUS_HEALTHY, 0]; + assert_eq!( + driver.on_transaction_complete(I2cCompletion::Complete), + LidarLiteV3Event::ReadingStatus + ); + driver.poll_measurement(timestamp).unwrap(); + assert_eq!( + driver.bus.transaction, + Transaction::Read { + address: LidarLiteV3Address::DEFAULT.as_7bit(), + length: 1 + } + ); + assert_eq!( + driver.on_transaction_complete(I2cCompletion::Complete), + LidarLiteV3Event::SelectingDistanceRegister + ); + driver.poll_measurement(timestamp).unwrap(); + assert_eq!( + driver.bus.transaction, + Transaction::Write { + address: LidarLiteV3Address::DEFAULT.as_7bit(), + bytes: [143, 0], + length: 1 + } + ); + assert_eq!( + driver.on_transaction_complete(I2cCompletion::Complete), + LidarLiteV3Event::ReadingMeasurement + ); + driver.bus.response = 291_u16.to_be_bytes(); + driver.poll_measurement(timestamp).unwrap(); + assert_eq!( + driver.bus.transaction, + Transaction::Read { + address: LidarLiteV3Address::DEFAULT.as_7bit(), + length: 2 + } + ); + assert_eq!( + driver.on_transaction_complete(I2cCompletion::Complete), + LidarLiteV3Event::Measurement(RangeSnapshot::new(timestamp, 2_910)) + ); + assert!(driver.is_ready()); + } + + #[test] + fn keeps_polling_while_the_device_is_busy() { + let mut driver = initialized(); + driver + .request_measurement( + Timestamp::from_microseconds(0), + AcquisitionMode::WithoutReceiverBiasCorrection, + ) + .unwrap(); + let _ = driver.on_transaction_complete(I2cCompletion::Complete); + driver + .poll_measurement(Timestamp::from_microseconds(0)) + .unwrap(); + driver.bus.response = [STATUS_HEALTHY | 1, 0]; + let _ = driver.on_transaction_complete(I2cCompletion::Complete); + driver + .poll_measurement(Timestamp::from_microseconds(0)) + .unwrap(); + assert_eq!( + driver.on_transaction_complete(I2cCompletion::Complete), + LidarLiteV3Event::MeasurementBusy + ); + assert!(!driver.is_ready()); + assert!( + driver + .poll_measurement(Timestamp::from_microseconds(0)) + .is_ok() + ); + } + + #[test] + fn recovers_from_a_measurement_that_remains_busy_too_long() { + let mut driver = initialized(); + driver + .request_measurement( + Timestamp::from_microseconds(0), + AcquisitionMode::WithReceiverBiasCorrection, + ) + .unwrap(); + let _ = driver.on_transaction_complete(I2cCompletion::Complete); + + assert_eq!( + driver.poll_measurement(Timestamp::from_microseconds( + MEASUREMENT_TIMEOUT_MICROSECONDS, + )), + Err(Error::MeasurementTimeout) + ); + assert!(driver.is_ready()); + assert_eq!(driver.bus.cancellations, 1); + } + + #[test] + fn reports_device_status_failures() { + let mut driver = initialized(); + driver + .request_measurement( + Timestamp::from_microseconds(0), + AcquisitionMode::WithReceiverBiasCorrection, + ) + .unwrap(); + let _ = driver.on_transaction_complete(I2cCompletion::Complete); + driver + .poll_measurement(Timestamp::from_microseconds(0)) + .unwrap(); + driver.bus.response = [STATUS_HEALTHY | STATUS_PROCESS_ERROR, 0]; + let _ = driver.on_transaction_complete(I2cCompletion::Complete); + driver + .poll_measurement(Timestamp::from_microseconds(0)) + .unwrap(); + assert_eq!( + driver.on_transaction_complete(I2cCompletion::Complete), + LidarLiteV3Event::Fault(Error::ProcessError) + ); + + driver + .request_measurement( + Timestamp::from_microseconds(0), + AcquisitionMode::WithReceiverBiasCorrection, + ) + .unwrap(); + let _ = driver.on_transaction_complete(I2cCompletion::Complete); + driver + .poll_measurement(Timestamp::from_microseconds(0)) + .unwrap(); + driver.bus.response = [0, 0]; + let _ = driver.on_transaction_complete(I2cCompletion::Complete); + driver + .poll_measurement(Timestamp::from_microseconds(0)) + .unwrap(); + assert_eq!( + driver.on_transaction_complete(I2cCompletion::Complete), + LidarLiteV3Event::Fault(Error::Unhealthy) + ); + + driver + .request_measurement( + Timestamp::from_microseconds(0), + AcquisitionMode::WithReceiverBiasCorrection, + ) + .unwrap(); + let _ = driver.on_transaction_complete(I2cCompletion::Complete); + driver + .poll_measurement(Timestamp::from_microseconds(0)) + .unwrap(); + driver.bus.response = [STATUS_HEALTHY | STATUS_INVALID_SIGNAL, 0]; + let _ = driver.on_transaction_complete(I2cCompletion::Complete); + driver + .poll_measurement(Timestamp::from_microseconds(0)) + .unwrap(); + assert_eq!( + driver.on_transaction_complete(I2cCompletion::Complete), + LidarLiteV3Event::Fault(Error::InvalidSignal) + ); + } + + #[test] + fn rejects_no_signal_and_preserves_the_full_raw_distance_range() { + let mut driver = initialized(); + driver + .request_measurement( + Timestamp::from_microseconds(0), + AcquisitionMode::WithReceiverBiasCorrection, + ) + .unwrap(); + let _ = driver.on_transaction_complete(I2cCompletion::Complete); + driver + .poll_measurement(Timestamp::from_microseconds(0)) + .unwrap(); + driver.bus.response = [STATUS_HEALTHY, 0]; + let _ = driver.on_transaction_complete(I2cCompletion::Complete); + let _ = driver.poll_measurement(Timestamp::from_microseconds(0)); + let _ = driver.on_transaction_complete(I2cCompletion::Complete); + let _ = driver.poll_measurement(Timestamp::from_microseconds(0)); + let _ = driver.on_transaction_complete(I2cCompletion::Complete); + driver.bus.response = 1_u16.to_be_bytes(); + let _ = driver.poll_measurement(Timestamp::from_microseconds(0)); + assert_eq!( + driver.on_transaction_complete(I2cCompletion::Complete), + LidarLiteV3Event::Fault(Error::NoSignal) + ); + + driver + .request_measurement( + Timestamp::from_microseconds(0), + AcquisitionMode::WithReceiverBiasCorrection, + ) + .unwrap(); + let _ = driver.on_transaction_complete(I2cCompletion::Complete); + driver + .poll_measurement(Timestamp::from_microseconds(0)) + .unwrap(); + driver.bus.response = [STATUS_HEALTHY, 0]; + let _ = driver.on_transaction_complete(I2cCompletion::Complete); + let _ = driver.poll_measurement(Timestamp::from_microseconds(0)); + let _ = driver.on_transaction_complete(I2cCompletion::Complete); + let _ = driver.poll_measurement(Timestamp::from_microseconds(0)); + let _ = driver.on_transaction_complete(I2cCompletion::Complete); + driver.bus.response = 0_u16.to_be_bytes(); + let _ = driver.poll_measurement(Timestamp::from_microseconds(0)); + assert_eq!( + driver.on_transaction_complete(I2cCompletion::Complete), + LidarLiteV3Event::Fault(Error::NoSignal) + ); + + driver + .request_measurement( + Timestamp::from_microseconds(0), + AcquisitionMode::WithReceiverBiasCorrection, + ) + .unwrap(); + let _ = driver.on_transaction_complete(I2cCompletion::Complete); + driver + .poll_measurement(Timestamp::from_microseconds(0)) + .unwrap(); + driver.bus.response = [STATUS_HEALTHY, 0]; + let _ = driver.on_transaction_complete(I2cCompletion::Complete); + let _ = driver.poll_measurement(Timestamp::from_microseconds(0)); + let _ = driver.on_transaction_complete(I2cCompletion::Complete); + let _ = driver.poll_measurement(Timestamp::from_microseconds(0)); + let _ = driver.on_transaction_complete(I2cCompletion::Complete); + driver.bus.response = u16::MAX.to_be_bytes(); + let _ = driver.poll_measurement(Timestamp::from_microseconds(0)); + assert_eq!( + driver.on_transaction_complete(I2cCompletion::Complete), + LidarLiteV3Event::Measurement(RangeSnapshot::new( + Timestamp::from_microseconds(0), + 655_350, + )) + ); + } + + #[test] + fn resets_to_idle_after_a_transport_fault() { + let mut driver = initialized(); + driver + .request_measurement( + Timestamp::from_microseconds(0), + AcquisitionMode::WithReceiverBiasCorrection, + ) + .unwrap(); + assert_eq!( + driver.on_transaction_complete(I2cCompletion::TransferFault), + LidarLiteV3Event::Fault(Error::TransferFault) + ); + assert!(driver.is_ready()); + } +} diff --git a/libs/kaptan-drivers/src/lidar_lite_v3/registers.rs b/libs/kaptan-drivers/src/lidar_lite_v3/registers.rs new file mode 100644 index 0000000..ffd7b05 --- /dev/null +++ b/libs/kaptan-drivers/src/lidar_lite_v3/registers.rs @@ -0,0 +1,14 @@ +//! LIDAR-Lite v3 I²C register definitions. + +pub(crate) const DEFAULT_ADDRESS: u8 = 98; +pub(crate) const ACQ_COMMAND: u8 = 0; +pub(crate) const STATUS: u8 = 1; +pub(crate) const FULL_DELAY_HIGH: u8 = 15; +pub(crate) const AUTO_INCREMENT: u8 = 128; +pub(crate) const STATUS_BUSY: u8 = 1; +pub(crate) const STATUS_INVALID_SIGNAL: u8 = 1 << 3; +pub(crate) const STATUS_HEALTHY: u8 = 1 << 5; +pub(crate) const STATUS_PROCESS_ERROR: u8 = 1 << 6; +pub(crate) const MEASURE_WITHOUT_RECEIVER_BIAS: u8 = 3; +pub(crate) const MEASURE_WITH_RECEIVER_BIAS: u8 = 4; +pub(crate) const MAX_NO_SIGNAL_DISTANCE_CENTIMETRES: u16 = 1;