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
58fn publish_range(result: Result<packets::RangePacket, errors::SensorError>) {
59    #[cfg(feature = "runtime-diagnostics")]
60    crate::runtime_diagnostics::record_signal_publish(
61        crate::runtime_diagnostics::SensorKind::Range,
62        RANGE_SIGNAL.signaled(),
63        result.is_err(),
64    );
65    RANGE_SIGNAL.signal(result);
66}
67
68pub struct Llv3hpSensor {
69    pub dev: I2cDevice<
70        'static,
71        CriticalSectionRawMutex,
72        I2c<'static, Async, embassy_stm32::i2c::mode::Master>,
73    >,
74}
75
76// Control Register List - Address Definitions
77const ACQ_COMMAND: u8 = 0x00; // Device command
78const STATUS: u8 = 0x01; // System status
79const SIG_COUNT_VAL: u8 = 0x02; // Maximum acquisition count
80const ACQ_CONFIG_REG: u8 = 0x04; // Acquisition mode control
81const DATA: u8 = 0x0F; // Distance measurement high byte
82const REF_COUNT_VAL: u8 = 0x12; // Reference acquisition count
83const THRESHOLD_BYPASS: u8 = 0x1C; // Peak detection threshold bypass
84const HEALTH_STATUS: u8 = 0x48; // Used to diagnose major hardware issues at initialization
85
86impl Llv3hpSensor {
87    async fn write_read(
88        &mut self,
89        address: u8,
90        register: &[u8],
91        data: &mut [u8],
92    ) -> Result<(), ()> {
93        match self.dev.write(address, register).await {
94            Err(_e) => return Err(()),
95            Ok(_) => {}
96        }
97
98        Timer::after(Duration::from_micros(0)).await;
99
100        // Read register
101        match self.dev.read(address, data).await {
102            Err(_e) => return Err(()),
103            Ok(_) => {}
104        }
105
106        Ok(())
107    }
108
109    pub async fn run(&mut self) {
110        const ADDRESS: u8 = 0x62;
111
112        // Check System Status Register
113        let mut status = [0u8; 1];
114        if self
115            .write_read(ADDRESS, &[STATUS], &mut status)
116            .await
117            .is_err()
118        {
119            publish_range(Err(errors::SensorError::GenericSensorError(
120                "LLV3HP Lidar failed: reading STATUS",
121            )));
122            return;
123        }
124        if self
125            .write_read(ADDRESS, &[STATUS], &mut status)
126            .await
127            .is_err()
128        {
129            publish_range(Err(errors::SensorError::GenericSensorError(
130                "LLV3HP Lidar failed: reading STATUS",
131            )));
132            return;
133        }
134
135        if (status[0] & 0x30) != 0x30 {
136            publish_range(Err(errors::SensorError::GenericSensorError(
137                "LLV3HP Lidar failed: bad STATUS",
138            )));
139            return;
140        }
141
142        // Check Health Status Register
143        let mut health = [0u8; 1];
144        if self
145            .write_read(ADDRESS, &[HEALTH_STATUS], &mut health)
146            .await
147            .is_err()
148        {
149            publish_range(Err(errors::SensorError::GenericSensorError(
150                "LLV3HP Lidar failed: reading HEALTH_STATUS",
151            )));
152            return;
153        }
154
155        if (health[0] & 0x17) != 0x17 {
156            publish_range(Err(errors::SensorError::GenericSensorError(
157                "LLV3HP Lidar failed: bad HEALTH_STATUS",
158            )));
159            return;
160        }
161
162        let sig_count_max: u8 = 0x80;
163        let acq_config_reg: u8 = 0x08;
164        let ref_count_max: u8 = 0x05;
165        let threshold_bypass: u8 = 0x00;
166
167        if self
168            .dev
169            .write(ADDRESS, &[SIG_COUNT_VAL, sig_count_max])
170            .await
171            .is_err()
172        {
173            publish_range(Err(errors::SensorError::GenericSensorError(
174                "LLV3HP Lidar failed: writing SIG_COUNT_VAL",
175            )));
176            return;
177        }
178        if self
179            .dev
180            .write(ADDRESS, &[ACQ_CONFIG_REG, acq_config_reg])
181            .await
182            .is_err()
183        {
184            publish_range(Err(errors::SensorError::GenericSensorError(
185                "LLV3HP Lidar failed: writing ACQ_CONFIG_REG",
186            )));
187            return;
188        }
189        if self
190            .dev
191            .write(ADDRESS, &[REF_COUNT_VAL, ref_count_max])
192            .await
193            .is_err()
194        {
195            publish_range(Err(errors::SensorError::GenericSensorError(
196                "LLV3HP Lidar failed: writing REF_COUNT_VAL",
197            )));
198            return;
199        }
200        if self
201            .dev
202            .write(ADDRESS, &[THRESHOLD_BYPASS, threshold_bypass])
203            .await
204            .is_err()
205        {
206            publish_range(Err(errors::SensorError::GenericSensorError(
207                "LLV3HP Lidar failed: writing THRESHOLD_BYPASS",
208            )));
209            return;
210        }
211
212        let loop_period = Duration::from_hz(100);
213        loop {
214            // Initiate another data read
215            if self
216                .dev
217                .write(ADDRESS, &[ACQ_COMMAND, 0x04u8])
218                .await
219                .is_err()
220            {
221                publish_range(Err(errors::SensorError::GenericSensorError(
222                    "LLV3HP Lidar failed: writing ACQ_COMMAND",
223                )));
224            }
225
226            let timestamp = synch_at(loop_period) + Duration::from_micros(5800);
227            Timer::at(timestamp).await;
228
229            // Read Data
230            let mut data = [0u8; 2];
231            if self.write_read(ADDRESS, &[DATA], &mut data).await.is_err() {
232                publish_range(Err(errors::SensorError::GenericSensorError(
233                    "LLV3HP Lidar failed: reading DATA",
234                )));
235            } else {
236                let urange = (u16::from(data[0]) << 8) | u16::from(data[1]); // cm
237                let range = f32::from(urange) / 100f32;
238
239                let timestamp_us = timestamp.as_micros();
240
241                let header = packets::RosflightPacketHeader {
242                    timestamp: timestamp_us,
243                    status: status[0] as u16,
244                };
245
246                let range_packet = packets::RangePacket {
247                    header,
248                    range,
249                    min_range: 0f32,
250                    max_range: 40f32,
251                    range_type: packets::RangeType::Lidar,
252                };
253                publish_range(Ok(range_packet)); // make data available for other tasks
254            }
255        }
256    }
257}
258
259#[embassy_executor::task]
260pub async fn task(mut llv3hp: Llv3hpSensor) {
261    llv3hp.run().await;
262}
stm_32/peripherals/llv3hp.rs

stm_32/peripherals/
llv3hp.rs
