stm_32/peripherals/
ms4525.rs

1// ******************************************************************************
2// * File     : platforms/stm_32/src/peripherals/ms4525.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 PITOT_SIGNAL: Signal<
54    CriticalSectionRawMutex,
55    Result<packets::PitotPacket, errors::SensorError>,
56> = Signal::<CriticalSectionRawMutex, Result<packets::PitotPacket, errors::SensorError>>::new();
57
58pub struct Ms4525Sensor {
59    pub dev: I2cDevice<
60        'static,
61        CriticalSectionRawMutex,
62        I2c<'static, Async, embassy_stm32::i2c::mode::Master>,
63    >,
64}
65
66impl Ms4525Sensor {
67    pub async fn run(&mut self) {
68        const ADDRESS: u8 = 0x28;
69        const NO_ERROR: u8 = 0x00;
70
71        // Start a read
72        let mut data = [0u8; 2];
73        if self.dev.read(ADDRESS, &mut data).await.is_err() {
74            PITOT_SIGNAL.signal(Err(errors::SensorError::GenericSensorError(
75                "MS4525 Pitot failed: reading data",
76            )));
77            return;
78        }
79
80        Timer::after(Duration::from_micros(2000)).await;
81
82        // Check if read OK.
83        let mut data = [0u8; 2];
84        if self.dev.read(ADDRESS, &mut data).await.is_err() {
85            PITOT_SIGNAL.signal(Err(errors::SensorError::GenericSensorError(
86                "MS4525 Pitot failed: reading data",
87            )));
88            return;
89        }
90
91        let status = (data[0] >> 6) & 0x03;
92        if status != NO_ERROR {
93            PITOT_SIGNAL.signal(Err(errors::SensorError::GenericSensorError(
94                "MS4525 Pitot failed: bad status",
95            )));
96            return;
97        }
98
99        let sample_period_us = Duration::from_hz(100).as_micros();
100        let loop_period = Duration::from_hz(400);
101        const PMAX: f64 = 6894.76; // (=-pmin) Pa
102        let mut sum_pressure: u32 = 0;
103        let mut sum_temperature: u32 = 0;
104        let mut sum_count: u32 = 0;
105
106        loop {
107            let timestamp = synch_at(loop_period);
108            Timer::at(timestamp).await; // Wait for top of timer
109
110            let mut data = [0u8; 4];
111            if self.dev.read(ADDRESS, &mut data).await.is_err() {
112                PITOT_SIGNAL.signal(Err(errors::SensorError::GenericSensorError(
113                    "MS4525 Pitot failed: reading data",
114                )));
115                continue;
116            }
117
118            let status = (data[0] >> 6) & 0x03;
119            if status == NO_ERROR {
120                let pressure_u32 = (u32::from(data[0]) & 0x3F) << 8 | u32::from(data[1]);
121                let temperature_u32 =
122                    (0xFFE0 & ((u32::from(data[2]) << 8) | u32::from(data[3]))) >> 5;
123
124                sum_pressure = sum_pressure + pressure_u32;
125                sum_temperature = sum_temperature + temperature_u32;
126                sum_count = sum_count + 1;
127            }
128
129            let timestamp_us = timestamp.as_micros();
130
131            if timestamp_us % sample_period_us == 0 {
132                if sum_count > 0 {
133                    let avg_pressure = ((f64::from(sum_pressure) / (sum_count as f64) - 1638.3f64)
134                        / 6553.2f64
135                        - 1.0f64)
136                        * PMAX;
137                    let avg_temperature: f64 =
138                        ((200.0f64 * f64::from(sum_temperature) / (sum_count as f64)) / 2047f64)
139                            - 50f64;
140
141                    let header = packets::RosflightPacketHeader {
142                        timestamp: timestamp_us,
143                        status: status as u16,
144                    };
145                    let pitot_packet = packets::PitotPacket {
146                        header,
147                        differential_pressure: avg_pressure as f32,
148                        temperature: avg_temperature as f32,
149                        ..Default::default()
150                    };
151                    PITOT_SIGNAL.signal(Ok(pitot_packet)); // make data available for other tasks.
152                } else {
153                    PITOT_SIGNAL.signal(Err(errors::SensorError::GenericSensorError(
154                        "MS4525 Pitot failed: no valid data",
155                    )));
156                }
157                // reset sum
158                //previous_timestamp_us = timestamp_us;
159                sum_count = 0u32;
160                sum_pressure = 0u32;
161                sum_temperature = 0u32;
162            }
163        }
164    }
165}
166
167#[embassy_executor::task]
168pub async fn task(mut ms4525: Ms4525Sensor) {
169    ms4525.run().await;
170}
stm_32/peripherals/ms4525.rs

stm_32/peripherals/
ms4525.rs
