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

stm_32/peripherals/
ms4525.rs
