cu_dps310/
lib.rs

1//! Copper source driver for the Infineon DPS310 digital barometric pressure sensor.
2//!
3//! This source expects an address-scoped I2C resource so the sensor address is
4//! defined by the board resource bundle (hardware mapping), not by component config.
5
6#![cfg_attr(not(feature = "std"), no_std)]
7
8extern crate alloc;
9
10use alloc::{format, string::String};
11use core::fmt::Debug;
12
13pub use cu_sensor_payloads::BarometerPayload;
14use cu29::prelude::*;
15use serde::{Deserialize, Serialize};
16
17// Registers
18const REG_PRS_B2: u8 = 0x00;
19const REG_PRS_CFG: u8 = 0x06;
20const REG_TMP_CFG: u8 = 0x07;
21const REG_MEAS_CFG: u8 = 0x08;
22const REG_CFG_REG: u8 = 0x09;
23const REG_RESET: u8 = 0x0C;
24const REG_ID: u8 = 0x0D;
25const REG_TMP_COEF_FIX_KEY1: u8 = 0x0E;
26const REG_TMP_COEF_FIX_KEY2: u8 = 0x0F;
27const REG_TMP_COEF_FIX_CTRL: u8 = 0x62;
28const REG_COEF: u8 = 0x10;
29const REG_COEF_SRCE: u8 = 0x28;
30
31// IDs
32const DPS310_ID_REV_AND_PROD: u8 = 0x10;
33const SPL07_003_CHIP_ID: u8 = 0x11;
34
35// Bitfields
36const RESET_SOFT_RST: u8 = 0x09;
37const MEAS_CFG_COEF_RDY: u8 = 1 << 7;
38const MEAS_CFG_SENSOR_RDY: u8 = 1 << 6;
39const MEAS_CFG_TMP_RDY: u8 = 1 << 5;
40const MEAS_CFG_MEAS_CTRL_IDLE: u8 = 0x00;
41const MEAS_CFG_MEAS_CTRL_TEMP_SING: u8 = 0x02;
42const MEAS_CFG_MEAS_CTRL_CONT_P_T: u8 = 0x07;
43
44const PRS_CFG_RATE_32HZ: u8 = 0x50;
45const PRS_CFG_PRC_16X: u8 = 0x04;
46const TMP_CFG_RATE_32HZ: u8 = 0x50;
47const TMP_CFG_PRC_16X: u8 = 0x04;
48const COEF_SRCE_TMP_COEF_SRCE: u8 = 0x80;
49
50const CFG_REG_T_SHIFT: u8 = 0x08;
51const CFG_REG_P_SHIFT: u8 = 0x04;
52
53// 16x OSR compensation scales
54const SCALE_KP_16X: f32 = 253_952.0;
55const SCALE_KT_16X: f32 = 253_952.0;
56
57const INIT_READY_POLL_LIMIT: usize = 10_000;
58const TEMP_READY_POLL_LIMIT: usize = 10_000;
59const DETECT_RETRY_LIMIT: usize = 5_000;
60const I2C_TRANSFER_RETRY_LIMIT: usize = 8;
61const RESET_SETTLE_SPINS: usize = 8_000_000;
62const COEF_CHUNK_READ_LEN: usize = 9;
63const OUTPUT_RATE_HZ: u64 = 30;
64const OUTPUT_PERIOD_NS: u64 = 1_000_000_000 / OUTPUT_RATE_HZ;
65const DRIVER_LOG_PERIOD_NS: u64 = 1_000_000_000;
66
67/// Address-scoped bus interface for DPS310 register I/O.
68///
69/// Implement this trait in your board bundle resource type so the driver does
70/// not carry or configure an I2C address.
71pub trait Dps310Bus: Send + Sync + 'static {
72    type Error: Debug + Send + 'static;
73
74    fn write(&mut self, write: &[u8]) -> Result<(), Self::Error>;
75    fn write_read(&mut self, write: &[u8], read: &mut [u8]) -> Result<(), Self::Error>;
76}
77
78#[derive(Debug, Clone, Copy, Serialize, Deserialize, Default)]
79struct CalibrationCoefficients {
80    c0: i16,
81    c1: i16,
82    c00: i32,
83    c10: i32,
84    c01: i16,
85    c11: i16,
86    c20: i16,
87    c21: i16,
88    c30: i16,
89    c31: i16,
90    c40: i16,
91}
92
93struct Dps310Driver<BUS>
94where
95    BUS: Dps310Bus,
96{
97    bus: BUS,
98    chip_id: u8,
99    calib: CalibrationCoefficients,
100}
101
102impl<BUS> Dps310Driver<BUS>
103where
104    BUS: Dps310Bus,
105{
106    fn new(bus: BUS) -> CuResult<Self> {
107        let mut driver = Self {
108            bus,
109            chip_id: 0,
110            calib: CalibrationCoefficients::default(),
111        };
112
113        driver.chip_id = driver.detect_chip_id()?;
114        debug!("dps310: detected id=0x{:02X}", driver.chip_id);
115        driver.soft_reset()?;
116        driver.wait_sensor_ready()?;
117        driver.read_calibration_coefficients()?;
118        driver.configure()?;
119        Ok(driver)
120    }
121
122    fn detect_chip_id(&mut self) -> CuResult<u8> {
123        let mut last_err: Option<&'static str> = None;
124        let mut last_id: Option<u8> = None;
125        let mut read_buf = [0u8; 1];
126        for _ in 0..DETECT_RETRY_LIMIT {
127            match self.bus.write_read(&[REG_ID], &mut read_buf) {
128                Ok(()) => {
129                    let id = read_buf[0];
130                    last_id = Some(id);
131                    if id == DPS310_ID_REV_AND_PROD || id == SPL07_003_CHIP_ID {
132                        return Ok(id);
133                    }
134                    last_err = Some("unexpected chip id");
135                }
136                Err(_) => {
137                    last_err = Some("i2c read error");
138                }
139            }
140            backoff_spin();
141        }
142        let last_id_str = match last_id {
143            Some(id) => format!("0x{:02X}", id),
144            None => String::from("none"),
145        };
146        Err(CuError::from(format!(
147            "dps310 detect failed: {} last_id={}",
148            last_err.unwrap_or("unknown"),
149            last_id_str
150        )))
151    }
152
153    fn soft_reset(&mut self) -> CuResult<()> {
154        self.write_reg_value(REG_RESET, RESET_SOFT_RST)
155            .map_err(|err| map_debug_error("dps310 soft reset", err))?;
156        // Match INAV/BF behavior: allow the sensor reset sequence to settle
157        // before polling status or touching configuration registers.
158        spin_wait(RESET_SETTLE_SPINS);
159        Ok(())
160    }
161
162    fn wait_sensor_ready(&mut self) -> CuResult<()> {
163        let mut last_status: Option<u8> = None;
164        let mut read_errors = 0u32;
165        for _ in 0..INIT_READY_POLL_LIMIT {
166            let status = match self.read_reg(REG_MEAS_CFG) {
167                Ok(v) => v,
168                Err(_) => {
169                    read_errors = read_errors.saturating_add(1);
170                    backoff_spin();
171                    continue;
172                }
173            };
174            last_status = Some(status);
175            let coeff_ready = (status & MEAS_CFG_COEF_RDY) != 0;
176            let sensor_ready = (status & MEAS_CFG_SENSOR_RDY) != 0;
177            if coeff_ready && sensor_ready {
178                return Ok(());
179            }
180            backoff_spin();
181        }
182        let last_status_str = match last_status {
183            Some(status) => format!("0x{:02X}", status),
184            None => String::from("none"),
185        };
186        Err(CuError::from(format!(
187            "dps310 init timeout: COEF_RDY/SENSOR_RDY not both set, last_meas_cfg={}, read_errors={}",
188            last_status_str, read_errors
189        )))
190    }
191
192    fn read_calibration_coefficients(&mut self) -> CuResult<()> {
193        let coef_len = if self.chip_id == SPL07_003_CHIP_ID {
194            22
195        } else {
196            18
197        };
198        let mut coef = [0u8; 22];
199        let mut loaded = false;
200
201        // Fast path: single contiguous read.
202        if self.read_reg_buf(REG_COEF, &mut coef[..coef_len]).is_ok() {
203            loaded = true;
204        }
205
206        // Fallback: chunked reads (Betaflight-style).
207        if !loaded {
208            let mut offset = 0usize;
209            loaded = true;
210            while offset < coef_len {
211                let chunk = core::cmp::min(COEF_CHUNK_READ_LEN, coef_len - offset);
212                let mut buf = [0u8; COEF_CHUNK_READ_LEN];
213                if self
214                    .read_reg_buf(REG_COEF + offset as u8, &mut buf[..chunk])
215                    .is_err()
216                {
217                    loaded = false;
218                    break;
219                }
220                coef[offset..offset + chunk].copy_from_slice(&buf[..chunk]);
221                offset += chunk;
222            }
223        }
224
225        // Final fallback: byte-wise reads for controllers/sensors that NACK bursts.
226        if !loaded {
227            debug!("dps310: coef burst read failed, falling back to byte reads");
228            for (i, slot) in coef[..coef_len].iter_mut().enumerate() {
229                *slot = self.read_reg(REG_COEF + i as u8)?;
230            }
231        }
232
233        self.calib = parse_coefficients(&coef, self.chip_id == SPL07_003_CHIP_ID);
234        debug!(
235            "dps310: coeff c00={} c10={} c01={} c11={} c20={} c21={} c30={}",
236            self.calib.c00,
237            self.calib.c10,
238            self.calib.c01,
239            self.calib.c11,
240            self.calib.c20,
241            self.calib.c21,
242            self.calib.c30
243        );
244        Ok(())
245    }
246
247    fn configure(&mut self) -> CuResult<()> {
248        self.write_reg_value(REG_MEAS_CFG, MEAS_CFG_MEAS_CTRL_IDLE)
249            .map_err(|err| map_debug_error("dps310 configure MEAS_CFG idle", err))?;
250
251        // INAV/BF compatibility workaround for known temperature coefficient issue.
252        self.write_reg_value(REG_TMP_COEF_FIX_KEY1, 0xA5)
253            .map_err(|err| map_debug_error("dps310 fix write 0x0E", err))?;
254        self.write_reg_value(REG_TMP_COEF_FIX_KEY2, 0x96)
255            .map_err(|err| map_debug_error("dps310 fix write 0x0F", err))?;
256        self.write_reg_value(REG_TMP_COEF_FIX_CTRL, 0x02)
257            .map_err(|err| map_debug_error("dps310 fix write 0x62", err))?;
258        self.write_reg_value(REG_TMP_COEF_FIX_KEY1, 0x00)
259            .map_err(|err| map_debug_error("dps310 fix clear 0x0E", err))?;
260        self.write_reg_value(REG_TMP_COEF_FIX_KEY2, 0x00)
261            .map_err(|err| map_debug_error("dps310 fix clear 0x0F", err))?;
262
263        self.write_reg_value(REG_MEAS_CFG, MEAS_CFG_MEAS_CTRL_TEMP_SING)
264            .map_err(|err| map_debug_error("dps310 configure MEAS_CFG temp single", err))?;
265        self.wait_temp_ready()?;
266
267        self.set_bits(REG_PRS_CFG, PRS_CFG_RATE_32HZ | PRS_CFG_PRC_16X)
268            .map_err(|err| map_debug_error("dps310 configure PRS_CFG", err))?;
269
270        let temp_coef_source = if self.chip_id == SPL07_003_CHIP_ID {
271            0
272        } else {
273            self.read_reg(REG_COEF_SRCE)? & COEF_SRCE_TMP_COEF_SRCE
274        };
275        self.set_bits(
276            REG_TMP_CFG,
277            TMP_CFG_RATE_32HZ | TMP_CFG_PRC_16X | temp_coef_source,
278        )
279        .map_err(|err| map_debug_error("dps310 configure TMP_CFG", err))?;
280
281        self.set_bits(REG_CFG_REG, CFG_REG_P_SHIFT | CFG_REG_T_SHIFT)
282            .map_err(|err| map_debug_error("dps310 configure CFG_REG", err))?;
283
284        self.write_reg_value(REG_MEAS_CFG, MEAS_CFG_MEAS_CTRL_CONT_P_T)
285            .map_err(|err| map_debug_error("dps310 configure MEAS_CFG cont", err))?;
286
287        debug!("dps310: configured background P+T mode (32Hz, 16x OSR)");
288        Ok(())
289    }
290
291    fn wait_temp_ready(&mut self) -> CuResult<()> {
292        let mut last_status: Option<u8> = None;
293        let mut read_errors = 0u32;
294        for _ in 0..TEMP_READY_POLL_LIMIT {
295            let status = match self.read_reg(REG_MEAS_CFG) {
296                Ok(v) => v,
297                Err(_) => {
298                    read_errors = read_errors.saturating_add(1);
299                    backoff_spin();
300                    continue;
301                }
302            };
303            last_status = Some(status);
304            if (status & MEAS_CFG_TMP_RDY) != 0 {
305                return Ok(());
306            }
307            backoff_spin();
308        }
309        let last_status_str = match last_status {
310            Some(status) => format!("0x{:02X}", status),
311            None => String::from("none"),
312        };
313        Err(CuError::from(format!(
314            "dps310 temp-ready timeout: last_meas_cfg={}, read_errors={}",
315            last_status_str, read_errors
316        )))
317    }
318
319    fn read_measure(&mut self) -> CuResult<BarometerPayload> {
320        let mut buf = [0u8; 6];
321        self.read_reg_buf(REG_PRS_B2, &mut buf)?;
322
323        let pressure_raw = twos_complement(
324            ((buf[0] as u32) << 16) | ((buf[1] as u32) << 8) | (buf[2] as u32),
325            24,
326        );
327        let temperature_raw = twos_complement(
328            ((buf[3] as u32) << 16) | ((buf[4] as u32) << 8) | (buf[5] as u32),
329            24,
330        );
331
332        let pressure = compensate_pressure_pa(
333            &self.calib,
334            pressure_raw,
335            temperature_raw,
336            self.chip_id == SPL07_003_CHIP_ID,
337        );
338        let temperature = compensate_temperature_c(&self.calib, temperature_raw);
339
340        Ok(BarometerPayload::from_raw(pressure, temperature))
341    }
342
343    fn read_reg(&mut self, reg: u8) -> CuResult<u8> {
344        let mut byte = [0u8; 1];
345        self.read_reg_buf(reg, &mut byte)?;
346        Ok(byte[0])
347    }
348
349    fn read_reg_buf(&mut self, reg: u8, read: &mut [u8]) -> CuResult<()> {
350        for _ in 0..I2C_TRANSFER_RETRY_LIMIT {
351            if self.bus.write_read(&[reg], read).is_ok() {
352                return Ok(());
353            }
354            backoff_spin();
355        }
356        Err(CuError::from(format!(
357            "dps310 i2c write_read failed: reg=0x{:02X} len={} retries={}",
358            reg,
359            read.len(),
360            I2C_TRANSFER_RETRY_LIMIT
361        )))
362    }
363
364    fn set_bits(&mut self, reg: u8, bits: u8) -> CuResult<()> {
365        let mut value = self.read_reg(reg)?;
366        if value & bits != bits {
367            value |= bits;
368            self.write_reg_value(reg, value)?;
369        }
370        Ok(())
371    }
372
373    fn write_reg_value(&mut self, reg: u8, value: u8) -> CuResult<()> {
374        for _ in 0..I2C_TRANSFER_RETRY_LIMIT {
375            if self.bus.write(&[reg, value]).is_ok() {
376                return Ok(());
377            }
378            backoff_spin();
379        }
380        Err(CuError::from(format!(
381            "dps310 i2c write failed: reg=0x{:02X} value=0x{:02X} retries={}",
382            reg, value, I2C_TRANSFER_RETRY_LIMIT
383        )))
384    }
385}
386
387fn backoff_spin() {
388    spin_wait(128);
389}
390
391fn spin_wait(iterations: usize) {
392    for _ in 0..iterations {
393        core::hint::spin_loop();
394    }
395}
396
397fn parse_coefficients(raw: &[u8; 22], is_spl07: bool) -> CalibrationCoefficients {
398    let c31 = if is_spl07 {
399        twos_complement(
400            ((raw[18] as u32) << 4) | (((raw[19] as u32) >> 4) & 0x0F),
401            12,
402        ) as i16
403    } else {
404        0
405    };
406    let c40 = if is_spl07 {
407        twos_complement((((raw[19] as u32) & 0x0F) << 8) | (raw[20] as u32), 12) as i16
408    } else {
409        0
410    };
411
412    CalibrationCoefficients {
413        c0: twos_complement(((raw[0] as u32) << 4) | (((raw[1] as u32) >> 4) & 0x0F), 12) as i16,
414        c1: twos_complement((((raw[1] as u32) & 0x0F) << 8) | (raw[2] as u32), 12) as i16,
415        c00: twos_complement(
416            ((raw[3] as u32) << 12) | ((raw[4] as u32) << 4) | (((raw[5] as u32) >> 4) & 0x0F),
417            20,
418        ),
419        c10: twos_complement(
420            (((raw[5] as u32) & 0x0F) << 16) | ((raw[6] as u32) << 8) | (raw[7] as u32),
421            20,
422        ),
423        c01: twos_complement(((raw[8] as u32) << 8) | (raw[9] as u32), 16) as i16,
424        c11: twos_complement(((raw[10] as u32) << 8) | (raw[11] as u32), 16) as i16,
425        c20: twos_complement(((raw[12] as u32) << 8) | (raw[13] as u32), 16) as i16,
426        c21: twos_complement(((raw[14] as u32) << 8) | (raw[15] as u32), 16) as i16,
427        c30: twos_complement(((raw[16] as u32) << 8) | (raw[17] as u32), 16) as i16,
428        c31,
429        c40,
430    }
431}
432
433fn compensate_temperature_c(calib: &CalibrationCoefficients, temperature_raw: i32) -> f32 {
434    let t_raw_sc = (temperature_raw as f32) / SCALE_KT_16X;
435    (calib.c0 as f32) * 0.5 + (calib.c1 as f32) * t_raw_sc
436}
437
438fn compensate_pressure_pa(
439    calib: &CalibrationCoefficients,
440    pressure_raw: i32,
441    temperature_raw: i32,
442    is_spl07: bool,
443) -> f32 {
444    let p_raw_sc = (pressure_raw as f32) / SCALE_KP_16X;
445    let t_raw_sc = (temperature_raw as f32) / SCALE_KT_16X;
446
447    let c00 = calib.c00 as f32;
448    let c10 = calib.c10 as f32;
449    let c20 = calib.c20 as f32;
450    let c30 = calib.c30 as f32;
451    let c01 = calib.c01 as f32;
452    let c11 = calib.c11 as f32;
453    let c21 = calib.c21 as f32;
454
455    if is_spl07 {
456        let c31 = calib.c31 as f32;
457        let c40 = calib.c40 as f32;
458        c00 + p_raw_sc * (c10 + p_raw_sc * (c20 + p_raw_sc * (c30 + p_raw_sc * c40)))
459            + t_raw_sc * c01
460            + t_raw_sc * p_raw_sc * (c11 + p_raw_sc * (c21 + p_raw_sc * c31))
461    } else {
462        c00 + p_raw_sc * (c10 + p_raw_sc * (c20 + p_raw_sc * c30))
463            + t_raw_sc * c01
464            + t_raw_sc * p_raw_sc * (c11 + p_raw_sc * c21)
465    }
466}
467
468fn twos_complement(raw: u32, bit_len: u8) -> i32 {
469    if raw & (1u32 << (bit_len - 1)) != 0 {
470        (raw as i32) - (1i32 << bit_len)
471    } else {
472        raw as i32
473    }
474}
475
476fn map_debug_error<E: Debug>(context: &str, err: E) -> CuError {
477    CuError::from(format!("{context}: {err:?}"))
478}
479
480resources!(for <BUS>
481where
482    BUS: Dps310Bus,
483{
484    i2c => Owned<BUS>,
485});
486
487/// Copper source task for DPS310.
488#[derive(Reflect)]
489#[reflect(no_field_bounds, from_reflect = false, type_path = false)]
490pub struct Dps310Source<BUS>
491where
492    BUS: Dps310Bus,
493{
494    #[reflect(ignore)]
495    driver: Dps310Driver<BUS>,
496    last_output_ns: Option<u64>,
497    last_log_ns: Option<u64>,
498}
499
500impl<BUS> TypePath for Dps310Source<BUS>
501where
502    BUS: Dps310Bus,
503{
504    fn type_path() -> &'static str {
505        "cu_dps310::Dps310Source"
506    }
507
508    fn short_type_path() -> &'static str {
509        "Dps310Source"
510    }
511
512    fn type_ident() -> Option<&'static str> {
513        Some("Dps310Source")
514    }
515
516    fn crate_name() -> Option<&'static str> {
517        Some("cu_dps310")
518    }
519
520    fn module_path() -> Option<&'static str> {
521        Some("")
522    }
523}
524
525impl<BUS> Freezable for Dps310Source<BUS> where BUS: Dps310Bus {}
526
527impl<BUS> CuSrcTask for Dps310Source<BUS>
528where
529    BUS: Dps310Bus,
530{
531    type Resources<'r> = Resources<BUS>;
532    type Output<'m> = output_msg!(BarometerPayload);
533
534    fn new(_config: Option<&ComponentConfig>, resources: Self::Resources<'_>) -> CuResult<Self>
535    where
536        Self: Sized,
537    {
538        let driver = Dps310Driver::new(resources.i2c.0)?;
539        Ok(Self {
540            driver,
541            last_output_ns: None,
542            last_log_ns: None,
543        })
544    }
545
546    fn start(&mut self, _ctx: &CuContext) -> CuResult<()> {
547        debug!("dps310: source started");
548        Ok(())
549    }
550
551    fn process<'o>(&mut self, ctx: &CuContext, output: &mut Self::Output<'o>) -> CuResult<()> {
552        let tov = ctx.now();
553        let now_ns = tov.as_nanos();
554
555        if let Some(last_output_ns) = self.last_output_ns
556            && now_ns.saturating_sub(last_output_ns) < OUTPUT_PERIOD_NS
557        {
558            output.clear_payload();
559            output.tov = Tov::None;
560            return Ok(());
561        }
562
563        let payload = self.driver.read_measure()?;
564        self.last_output_ns = Some(now_ns);
565
566        if self
567            .last_log_ns
568            .is_none_or(|last_log_ns| now_ns.saturating_sub(last_log_ns) >= DRIVER_LOG_PERIOD_NS)
569        {
570            self.last_log_ns = Some(now_ns);
571            info!(
572                "dps310: pressure_pa={} temp_c={} rate_hz={}",
573                payload.pressure.value, payload.temperature.value, OUTPUT_RATE_HZ
574            );
575        }
576
577        output.tov = Tov::Time(tov);
578        output.set_payload(payload);
579        Ok(())
580    }
581}
cu_dps310/lib.rs

cu_dps310/
lib.rs
