stm_32/peripherals/
llv3hp.rs

1// ******************************************************************************
2// * File     : platforms/stm_32/src/peripherals/llv3hp.rs
3// * Date     : June 28, 2026
4// ******************************************************************************
5// *
6// * Copyright (c) 2023, AeroVironment, Inc.
7// * All rights reserved.
8// *
9// * Redistribution and use in source and binary forms, with or without
10// * modification, are permitted provided that the following conditions are met:
11// *
12// * 1.Redistributions of source code must retain the above copyright notice, this
13// * list of conditions and the following disclaimer.
14// *
15// * 2.Redistributions in binary form must reproduce the above copyright notice,
16// * this list of conditions and the following disclaimer in the documentation
17// * and/or other materials provided with the distribution.
18// *
19// * 3.Neither the name of the copyright holder nor the names of its
20// * contributors may be used to endorse or promote products derived from
21// * this software without specific prior written permission.
22// *
23// * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
24// * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
25// * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
26// * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
27// * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
28// * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
29// * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30// * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
31// * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
32// * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
33// *
34// ******************************************************************************
35
36pub use embassy_stm32::mode::Async;
37pub use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
38pub use embassy_sync::signal::Signal;
39use veloxity_core::{errors, packets};
40
41// I2C Specific
42use embassy_embedded_hal::shared_bus::asynch::i2c::I2cDevice;
43use embassy_stm32::i2c::I2c;
44use embedded_hal_async::i2c::I2c as _;
45
46// Polled Sensors
47use crate::synch_at;
48use embassy_time::Duration;
49use embassy_time::Timer;
50
51// Other
52
53pub static RANGE_SIGNAL: Signal<
54    CriticalSectionRawMutex,
55    Result<packets::RangePacket, errors::SensorError>,
56> = Signal::<CriticalSectionRawMutex, Result<packets::RangePacket, errors::SensorError>>::new();
57
58pub struct Llv3hpSensor {
59    pub dev: I2cDevice<
60        'static,
61        CriticalSectionRawMutex,
62        I2c<'static, Async, embassy_stm32::i2c::mode::Master>,
63    >,
64}
65
66// Control Register List - Address Definitions
67const ACQ_COMMAND: u8 = 0x00; // Device command
68const STATUS: u8 = 0x01; // System status
69const SIG_COUNT_VAL: u8 = 0x02; // Maximum acquisition count
70const ACQ_CONFIG_REG: u8 = 0x04; // Acquisition mode control
71const DATA: u8 = 0x0F; // Distance measurement high byte
72const REF_COUNT_VAL: u8 = 0x12; // Reference acquisition count
73const THRESHOLD_BYPASS: u8 = 0x1C; // Peak detection threshold bypass
74const HEALTH_STATUS: u8 = 0x48; // Used to diagnose major hardware issues at initialization
75
76impl Llv3hpSensor {
77    async fn write_read(
78        &mut self,
79        address: u8,
80        register: &[u8],
81        data: &mut [u8],
82    ) -> Result<(), ()> {
83        match self.dev.write(address, register).await {
84            Err(_e) => return Err(()),
85            Ok(_) => {}
86        }
87
88        Timer::after(Duration::from_micros(0)).await;
89
90        // Read register
91        match self.dev.read(address, data).await {
92            Err(_e) => return Err(()),
93            Ok(_) => {}
94        }
95
96        Ok(())
97    }
98
99    pub async fn run(&mut self) {
100        const ADDRESS: u8 = 0x62;
101
102        // Check System Status Register
103        let mut status = [0u8; 1];
104        if self
105            .write_read(ADDRESS, &[STATUS], &mut status)
106            .await
107            .is_err()
108        {
109            RANGE_SIGNAL.signal(Err(errors::SensorError::GenericSensorError(
110                "LLV3HP Lidar failed: reading STATUS",
111            )));
112            return;
113        }
114        if self
115            .write_read(ADDRESS, &[STATUS], &mut status)
116            .await
117            .is_err()
118        {
119            RANGE_SIGNAL.signal(Err(errors::SensorError::GenericSensorError(
120                "LLV3HP Lidar failed: reading STATUS",
121            )));
122            return;
123        }
124
125        if (status[0] & 0x30) != 0x30 {
126            RANGE_SIGNAL.signal(Err(errors::SensorError::GenericSensorError(
127                "LLV3HP Lidar failed: bad STATUS",
128            )));
129            return;
130        }
131
132        // Check Health Status Register
133        let mut health = [0u8; 1];
134        if self
135            .write_read(ADDRESS, &[HEALTH_STATUS], &mut health)
136            .await
137            .is_err()
138        {
139            RANGE_SIGNAL.signal(Err(errors::SensorError::GenericSensorError(
140                "LLV3HP Lidar failed: reading HEALTH_STATUS",
141            )));
142            return;
143        }
144
145        if (health[0] & 0x17) != 0x17 {
146            RANGE_SIGNAL.signal(Err(errors::SensorError::GenericSensorError(
147                "LLV3HP Lidar failed: bad HEALTH_STATUS",
148            )));
149            return;
150        }
151
152        let sig_count_max: u8 = 0x80;
153        let acq_config_reg: u8 = 0x08;
154        let ref_count_max: u8 = 0x05;
155        let threshold_bypass: u8 = 0x00;
156
157        if self
158            .dev
159            .write(ADDRESS, &[SIG_COUNT_VAL, sig_count_max])
160            .await
161            .is_err()
162        {
163            RANGE_SIGNAL.signal(Err(errors::SensorError::GenericSensorError(
164                "LLV3HP Lidar failed: writing SIG_COUNT_VAL",
165            )));
166            return;
167        }
168        if self
169            .dev
170            .write(ADDRESS, &[ACQ_CONFIG_REG, acq_config_reg])
171            .await
172            .is_err()
173        {
174            RANGE_SIGNAL.signal(Err(errors::SensorError::GenericSensorError(
175                "LLV3HP Lidar failed: writing ACQ_CONFIG_REG",
176            )));
177            return;
178        }
179        if self
180            .dev
181            .write(ADDRESS, &[REF_COUNT_VAL, ref_count_max])
182            .await
183            .is_err()
184        {
185            RANGE_SIGNAL.signal(Err(errors::SensorError::GenericSensorError(
186                "LLV3HP Lidar failed: writing REF_COUNT_VAL",
187            )));
188            return;
189        }
190        if self
191            .dev
192            .write(ADDRESS, &[THRESHOLD_BYPASS, threshold_bypass])
193            .await
194            .is_err()
195        {
196            RANGE_SIGNAL.signal(Err(errors::SensorError::GenericSensorError(
197                "LLV3HP Lidar failed: writing THRESHOLD_BYPASS",
198            )));
199            return;
200        }
201
202        let loop_period = Duration::from_hz(100);
203        loop {
204            // Initiate another data read
205            if self
206                .dev
207                .write(ADDRESS, &[ACQ_COMMAND, 0x04u8])
208                .await
209                .is_err()
210            {
211                RANGE_SIGNAL.signal(Err(errors::SensorError::GenericSensorError(
212                    "LLV3HP Lidar failed: writing ACQ_COMMAND",
213                )));
214            }
215
216            let timestamp = synch_at(loop_period) + Duration::from_micros(5800);
217            Timer::at(timestamp).await;
218
219            // Read Data
220            let mut data = [0u8; 2];
221            if self.write_read(ADDRESS, &[DATA], &mut data).await.is_err() {
222                RANGE_SIGNAL.signal(Err(errors::SensorError::GenericSensorError(
223                    "LLV3HP Lidar failed: reading DATA",
224                )));
225            } else {
226                let urange = (u16::from(data[0]) << 8) | u16::from(data[1]); // cm
227                let range = f32::from(urange) / 100f32;
228
229                let timestamp_us = timestamp.as_micros();
230
231                let header = packets::RosflightPacketHeader {
232                    timestamp: timestamp_us,
233                    status: status[0] as u16,
234                };
235
236                let range_packet = packets::RangePacket {
237                    header,
238                    range,
239                    min_range: 0f32,
240                    max_range: 40f32,
241                    range_type: packets::RangeType::Lidar,
242                };
243                RANGE_SIGNAL.signal(Ok(range_packet)); // make data available for other tasks
244            }
245        }
246    }
247}
248
249#[embassy_executor::task]
250pub async fn task(mut llv3hp: Llv3hpSensor) {
251    llv3hp.run().await;
252}
stm_32/peripherals/llv3hp.rs

stm_32/peripherals/
llv3hp.rs
