ld06_embed/
scan_types.rs

1/// A range reading from the sensor.
2#[derive(Copy, Clone, Debug, Default, Eq, PartialEq)]
3pub struct Range {
4    /// The distance from the unit, in mm.
5    pub dist: u16,
6    /// The intensity of the scan. 200 is 'average'.
7    pub confidence: u8,
8}
9
10/// A single scan packet from the Lidar.
11///
12/// All angles are with clockwise respect to the arrow on the top of the unit.
13#[derive(Copy, Clone, Debug, Default)]
14pub struct PartialScan {
15    /// The rotational speed of the unit, in degrees per second.
16    pub radar_speed: u16,
17    /// The starting angle of this scan, in degrees.
18    pub start_angle: f32,
19    /// The measured ranges.
20    ///
21    /// The first range angle is at [start_angle].
22    pub data: [Range; 12],
23    /// The ending angle of this scan, in degrees.
24    pub end_angle: f32,
25    /// The timestamp of this scan, in ms. This will roll over at 30000.
26    pub stamp: u16,
27    /// The CRC check from the lidar.
28    pub crc: u8,
29}
30
31impl PartialScan {
32    /// Gets the angular step per range reading.
33    pub fn get_step(&self) -> f32 {
34        // This is all to get the mod as an int to avoid floating point errors
35        let diff =
36            ((self.end_angle * 100.0) as usize + 36000 - (self.start_angle * 100.0) as usize) % 36000;
37        (diff / (self.data.len() - 1)) as f32 / 100.0
38    }
39
40    /// Calculates the angle the nth reading was at in this packet.
41    /// The reading number in this case is 0 indexed.
42    pub fn get_angle_of_reading(&self, reading_num: u16) -> f32 {
43        let mut angle = self.start_angle + self.get_step() * (reading_num) as f32;
44        if angle >= 360.0 {
45            angle -= 360.0;
46        }
47        angle
48    }
49
50    /// Translates the range from polar coordinates in terms of the LiDAR to polar coordinates in the standard format.
51    /// In practice, this is rotating all points by 90 degrees clockwise, then reflecting.
52    /// All angles are still in degrees.
53    pub fn get_range_in_polar(&self, reading_num: u16) -> (f32, f32) {
54        let range = self.data[reading_num as usize].dist as f32;
55        let angle = self.get_angle_of_reading(reading_num);
56
57        let mut p_deg = 90.0 - angle;
58        if p_deg < 0.0 {
59            while p_deg < 0.0 {
60                p_deg += 360.0;
61            }
62        }
63        (range, p_deg)
64    }
65}
ld06_embed/scan_types.rs

ld06_embed/
scan_types.rs
