1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88

//! # Ultrasonic Distance Sensor Module
//!
//! This module provides support for ultrasonic distance sensors, allowing for
//! the measurement of distances by emitting ultrasonic pulses and measuring the
//! time taken for the echo to return.

const SOUND_SPEED: f32 = 331.3; // Base speed of sound in air at 0 degrees Celsius in m/s
const SOUND_SPEED_INC_OVER_TEMP: f32 = 0.606; // Increase in the speed of sound per degree Celsius

use embedded_hal::{
    blocking::delay::DelayUs,
    digital::v2::{InputPin, OutputPin},
};
use esp_hal::delay::Delay;
#[cfg(not(feature = "esp32"))]
use esp_hal::systimer::SystemTimer;
#[cfg(all(feature = "esp32", feature = "wifi"))]
use esp_wifi::current_millis;

/// Represents an ultrasonic distance sensor with trigger and echo pins
pub struct USDistanceSensor<TriggerPin, EchoPin>
where
    TriggerPin: OutputPin<Error = core::convert::Infallible>,
    EchoPin: InputPin<Error = core::convert::Infallible>,
{
    trigger: TriggerPin,
    echo: EchoPin,
    delay: Delay,
}

impl<TriggerPin, EchoPin> USDistanceSensor<TriggerPin, EchoPin>
where
    TriggerPin: OutputPin<Error = core::convert::Infallible>,
    EchoPin: InputPin<Error = core::convert::Infallible>,
{
    /// Initializes a new ultrasonic distance sensor.
    ///
    /// # Arguments
    /// * `trigger` - The output pin used to trigger the sensor.
    /// * `echo` - The input pin used to read the echo signal.
    /// * `delay` - Delay provider for timing the trigger pulse.
    ///
    /// # Returns
    /// A new instance of `USDistanceSensor`.
    pub fn create_on_pins(mut trigger: TriggerPin, echo: EchoPin, delay: Delay) -> Self {
        trigger.set_low().unwrap();
        USDistanceSensor {
            trigger,
            echo,
            delay,
        }
    }

    /// Measures the distance to an object by sending an ultrasonic pulse and
    /// measuring the time taken for the echo to return.
    /// !!! For esp32 should only be built with esp-wifi ("esp32-wifi" feature)
    ///
    /// # Arguments
    /// * `ambient_temp` - The ambient temperature in degrees Celsius, used to
    ///   adjust the speed of sound.
    ///
    /// # Returns
    /// The measured distance in meters.
    pub fn get_distance(&mut self, ambient_temp: f32) -> f32 {
        let sound_speed = SOUND_SPEED + (SOUND_SPEED_INC_OVER_TEMP * ambient_temp);
        self.trigger.set_high().unwrap();
        self.delay.delay_us(10 as u32);
        self.trigger.set_low().unwrap();

        while self.echo.is_low().unwrap() {}
        #[cfg(not(feature = "esp32"))]
        // for esp32 should only be built with esp-wifi ("esp32-wifi" feature)
        let start_timestamp = SystemTimer::now();
        #[cfg(all(feature = "esp32", feature = "wifi"))]
        let start_timestamp = current_millis();
        while self.echo.is_high().unwrap() {}
        #[cfg(not(feature = "esp32"))]
        let end_timestamp = SystemTimer::now();
        #[cfg(all(feature = "esp32", feature = "wifi"))]
        let end_timestamp = current_millis();

        sound_speed * ((end_timestamp as f32 - start_timestamp as f32) / 10000.0) / 2.0
    }
}

// UnifiedData trait can't be implemented here because ambient temperature is
// requred for distance measurement, which means it won't be matching the `fn
// read` signature.