embedded_camsense_x1/
types.rs

1use crate::constants::{
2    NUMBER_OF_POINTS_PER_MEASUREMENT, NUMBER_OF_POINTS_PER_SCAN, PAYLOAD_SIZE_IN_BYTES,
3};
4
5/// Errors that can occur during Camsense-X1 communication or packet parsing.
6#[derive(Clone, Copy, Debug)]
7#[cfg_attr(feature = "defmt", derive(defmt::Format))]
8pub enum Error<E> {
9    /// Wrapped UART Error
10    UART(E),
11    /// Checksum mismatch error
12    ChecksumMismatch(u32, u32),
13    /// Other error
14    Other,
15}
16
17/// Raw distance measurement extracted from a single LiDAR packet.
18#[derive(Clone, Copy, Debug)]
19#[cfg_attr(feature = "defmt", derive(defmt::Format))]
20pub struct RawDistance {
21    /// 14-bit unsigned distance in mm, top 2 bits masked off
22    value: u16,
23    /// Signal quality/intensity indicator (0-255).
24    quality: u8,
25    /// true = invalid return (bit 7 of high byte)
26    flag: bool,
27}
28
29/// Parsed raw data from a single 36-byte LiDAR packet.
30#[derive(Clone, Copy, Debug)]
31#[cfg_attr(feature = "defmt", derive(defmt::Format))]
32pub struct RawMeasurement {
33    pub speed: u16,
34    /// Start angle of measurement in degrees
35    pub start_angle: u16,
36    /// End angle of measurement in degrees
37    pub end_angle: u16,
38    /// Array of distance measurements
39    pub distances: [RawDistance; NUMBER_OF_POINTS_PER_MEASUREMENT],
40    /// 16-bit Checksum
41    pub checksum: u16,
42}
43
44/// Verifies the Camsense-X1 checksum.
45/// Accepts the 36-byte payload (header, data, checksum).
46///
47/// The exact algorithm was taken from the official Camsense-X1 C++ SDK:
48/// https://github.com/camsense/SDK_V3.0/blob/17e0264302e2ca4cf14d5402af7437d16a37ab95/src/base/ReadParsePackage.cpp#L148
49#[inline]
50pub fn check_lidar_checksum(data: &[u8; PAYLOAD_SIZE_IN_BYTES]) -> Result<(), Error<()>> {
51    let mut accumulator: u32 = 0;
52
53    // Process all words in the slice
54    let num_data_words = data.len() / 2 - 1;
55    for i in 0..num_data_words {
56        let word = u16::from_le_bytes([data[2 * i], data[2 * i + 1]]);
57        accumulator = (accumulator << 1) + word as u32;
58    }
59
60    // 15-bit folding: equivalent to acc % 32767
61    let computed_checksum = ((accumulator & 0x7FFF) + (accumulator >> 15)) & 0x7FFF;
62
63    // Compare with the last word (checksum) in Little-Endian
64    let expected_checksum = u16::from_le_bytes([data[data.len() - 2], data[data.len() - 1]]) as u32;
65
66    if computed_checksum == expected_checksum {
67        Ok(())
68    } else {
69        Err(Error::ChecksumMismatch(
70            expected_checksum,
71            computed_checksum,
72        ))
73    }
74}
75
76impl TryFrom<[u8; 36]> for RawMeasurement {
77    type Error = Error<()>; // No UART context during pure byte parsing
78    fn try_from(data: [u8; PAYLOAD_SIZE_IN_BYTES]) -> Result<Self, Self::Error> {
79        // Validate checksum
80        check_lidar_checksum(&data)?;
81
82        let speed = u16::from_le_bytes([data[4], data[5]]);
83        let start_angle = u16::from_le_bytes([data[6], data[7]]);
84        let end_angle = u16::from_le_bytes([data[32], data[33]]);
85        let checksum = u16::from_le_bytes([data[34], data[35]]);
86
87        let mut distances = [RawDistance {
88            value: 0,
89            quality: 0,
90            flag: false,
91        }; 8];
92        for i in 0..8 {
93            let distance_bytes = [data[8 + i * 3], data[9 + i * 3], data[10 + i * 3]];
94            // Mask left-most bit of second byte
95            let value_bytes = [distance_bytes[0], distance_bytes[1] & 0x3F];
96            let value = u16::from_le_bytes(value_bytes);
97            let quality = distance_bytes[2];
98            // Flag this value as invalid if the flag bit is set
99            let flag = (distance_bytes[1] >> 7) & 0x01 != 0;
100
101            let distance = RawDistance {
102                value,
103                quality,
104                flag,
105            };
106            distances[i] = distance;
107        }
108
109        Ok(Self {
110            speed,
111            start_angle,
112            end_angle,
113            distances,
114            checksum,
115        })
116    }
117}
118
119/// A single validated LiDAR point with computed angle.
120#[derive(Clone, Copy, Default, Debug)]
121#[cfg_attr(feature = "defmt", derive(defmt::Format))]
122pub struct Point {
123    pub distance: u16,
124    pub angle: f32,
125}
126
127/// A single partial scan containing up to [`NUMBER_OF_POINTS_PER_MEASUREMENT`] valid points.
128///
129/// Represents the decoded and filtered data from one raw LiDAR packet.
130/// Invalid points, zero-quality returns, and flagged measurements are represented as None.
131#[derive(Clone, Copy, Debug)]
132#[cfg_attr(feature = "defmt", derive(defmt::Format))]
133pub struct PartialScan {
134    /// Rotation frequency in Hz.
135    pub frequency: f32,
136    /// Start angle of the packet in degrees.
137    pub start_angle: f32,
138    /// End angle of the packet in degrees.
139    pub end_angle: f32,
140    /// Array of [`NUMBER_OF_POINTS_PER_MEASUREMENT`] points within this partial scan.
141    pub points: [Option<Point>; NUMBER_OF_POINTS_PER_MEASUREMENT],
142}
143
144impl From<(RawMeasurement, f32)> for PartialScan {
145    fn from((raw, angle_offset): (RawMeasurement, f32)) -> Self {
146        let frequency = raw.speed as f32 / 3840.0;
147        let start_angle = raw.start_angle as f32 / 64.0 - 640.0;
148        let end_angle = raw.end_angle as f32 / 64.0 - 640.0;
149        let step = if end_angle > start_angle {
150            (end_angle - start_angle) / 7.0
151        } else {
152            (end_angle - (start_angle - 360.0)) / 7.0
153        };
154
155        let mut points = [None; NUMBER_OF_POINTS_PER_MEASUREMENT];
156        for (i, raw_distance) in raw.distances.iter().enumerate() {
157            if raw_distance.flag || raw_distance.quality == 0 || raw_distance.value == 0 {
158                continue;
159            }
160
161            let angle = (start_angle + step * i as f32 + angle_offset) % 360.0;
162            points[i] = Some(Point {
163                distance: raw_distance.value,
164                angle,
165            });
166        }
167        Self {
168            frequency,
169            start_angle,
170            end_angle,
171            points,
172        }
173    }
174}
175
176/// A complete 360° LiDAR scan.
177///
178/// Aggregates multiple partial scans into a fixed-size array of [`NUMBER_OF_POINTS_PER_SCAN`] points.
179#[derive(Clone, Copy, Debug)]
180#[cfg_attr(feature = "defmt", derive(defmt::Format))]
181pub struct Scan {
182    /// Full array of [`NUMBER_OF_POINTS_PER_SCAN`] points. `None` indicates no valid return at that angle/index.
183    pub points: [Option<Point>; NUMBER_OF_POINTS_PER_SCAN],
184}
embedded_camsense_x1/types.rs

embedded_camsense_x1/
types.rs
