Branch data Line data Source code
1 : : #include "us_driver.hpp"
2 : :
3 : : #define LOG_LOCAL_LEVEL ESP_LOG_DEBUG
4 : : #include "esp_log.h"
5 : :
6 : : static const char *TAG = "UsDriver";
7 : :
8 : 19 : UsDriver::UsDriver(
9 : : std::shared_ptr<IGpioHAL> gpio_hal,
10 : : std::shared_ptr<ITimerHAL> timer_hal,
11 : : gpio_num_t trig_pin,
12 : 19 : gpio_num_t echo_pin)
13 : 19 : : gpio_hal_(gpio_hal)
14 [ + - ]: 19 : , timer_hal_(timer_hal)
15 : 19 : , trig_pin_(trig_pin)
16 [ + - ]: 19 : , echo_pin_(echo_pin)
17 : : {
18 : 19 : }
19 : :
20 : 9 : esp_err_t UsDriver::init(uint16_t warmup_time_ms)
21 : : {
22 : 9 : ESP_LOGD(TAG, "Initializing UsDriver: TRIG=%d, ECHO=%d", trig_pin_, echo_pin_);
23 : :
24 : 9 : esp_err_t ret;
25 : :
26 : : // Configure TRIG pin as output
27 : :
28 : 9 : ret = gpio_hal_->reset_pin(trig_pin_);
29 [ + + ]: 9 : if (ret != ESP_OK)
30 : : return ret;
31 : :
32 : 8 : gpio_config_t trig_conf = {
33 : 8 : .pin_bit_mask = 1ULL << trig_pin_,
34 : : .mode = GPIO_MODE_OUTPUT,
35 : : .pull_up_en = GPIO_PULLUP_DISABLE,
36 : : .pull_down_en = GPIO_PULLDOWN_DISABLE,
37 : : .intr_type = GPIO_INTR_DISABLE,
38 : 8 : };
39 : 8 : ret = gpio_hal_->config(trig_conf);
40 [ + + ]: 8 : if (ret != ESP_OK)
41 : : return ret;
42 : :
43 : 7 : ret = gpio_hal_->set_level(trig_pin_, 0);
44 [ + + ]: 7 : if (ret != ESP_OK)
45 : : return ret;
46 : :
47 : : // Configure ECHO pin as input
48 : 6 : ret = gpio_hal_->reset_pin(echo_pin_);
49 [ + + ]: 6 : if (ret != ESP_OK)
50 : : return ret;
51 : :
52 : 5 : gpio_config_t echo_conf = {
53 : 5 : .pin_bit_mask = 1ULL << echo_pin_,
54 : : .mode = GPIO_MODE_INPUT,
55 : : .pull_up_en = GPIO_PULLUP_DISABLE,
56 : : .pull_down_en = GPIO_PULLDOWN_DISABLE,
57 : : .intr_type = GPIO_INTR_DISABLE,
58 : 5 : };
59 : 5 : ret = gpio_hal_->config(echo_conf);
60 [ + + ]: 5 : if (ret != ESP_OK)
61 : : return ret;
62 : :
63 : : // Pulse echo low to clear any residual state
64 : 4 : ret = gpio_hal_->set_direction(echo_pin_, GPIO_MODE_OUTPUT);
65 [ + + ]: 4 : if (ret != ESP_OK)
66 : : return ret;
67 : :
68 : 3 : ret = gpio_hal_->set_level(echo_pin_, 0);
69 [ + + ]: 3 : if (ret != ESP_OK)
70 : : return ret;
71 : :
72 : : // Warmup: wait for the sensor to stabilize before the first ping
73 [ + + ]: 2 : if (warmup_time_ms > 0) {
74 : 1 : ESP_LOGD(TAG, "Warming up for %d ms", warmup_time_ms);
75 : 1 : ret = timer_hal_->delay_ms(warmup_time_ms);
76 [ + - ]: 1 : if (ret != ESP_OK)
77 : 1 : return ret;
78 : : }
79 : :
80 : : return ESP_OK;
81 : : }
82 : :
83 : 5 : esp_err_t UsDriver::deinit()
84 : : {
85 : : // Reset pins to a safe state
86 : 5 : esp_err_t ret;
87 : :
88 : 5 : ret = gpio_hal_->set_level(trig_pin_, 0);
89 [ + + ]: 5 : if (ret != ESP_OK)
90 : : return ret;
91 : 4 : ret = gpio_hal_->reset_pin(trig_pin_);
92 [ + + ]: 4 : if (ret != ESP_OK)
93 : : return ret;
94 : :
95 : 3 : ret = gpio_hal_->set_level(echo_pin_, 0);
96 [ + + ]: 3 : if (ret != ESP_OK)
97 : : return ret;
98 : 2 : ret = gpio_hal_->reset_pin(echo_pin_);
99 [ + + ]: 2 : if (ret != ESP_OK)
100 : 1 : return ret;
101 : :
102 : : return ESP_OK;
103 : : }
104 : :
105 : 22 : Reading UsDriver::ping_once(const UsConfig &cfg)
106 : : {
107 : : // 1. Prepare: set ECHO as output low to clear residual state
108 [ + + ]: 22 : if (gpio_hal_->set_direction(echo_pin_, GPIO_MODE_OUTPUT) != ESP_OK)
109 : 1 : return {UsResult::HW_FAULT, 0.0f};
110 [ + + ]: 21 : if (gpio_hal_->set_level(echo_pin_, 0) != ESP_OK)
111 : 1 : return {UsResult::HW_FAULT, 0.0f};
112 : :
113 : : // 2. Switch ECHO to input for measurement
114 [ + + ]: 20 : if (gpio_hal_->set_direction(echo_pin_, GPIO_MODE_INPUT) != ESP_OK)
115 : 1 : return {UsResult::HW_FAULT, 0.0f};
116 : :
117 : : // 3. Check if ECHO is stuck HIGH before triggering
118 [ + + ]: 19 : if (is_echo_stuck())
119 : 1 : return {UsResult::ECHO_STUCK, 0.0f};
120 : :
121 : : // 4. Send trigger pulse
122 : 18 : esp_err_t ret = trigger(cfg.ping_duration_us);
123 [ + + ]: 18 : if (ret != ESP_OK)
124 : 3 : return {UsResult::HW_FAULT, 0.0f};
125 : :
126 : : // 5. Wait for rising edge (start of echo pulse)
127 : 15 : ret = wait_rising_edge(cfg.timeout_us);
128 [ + + ]: 15 : if (ret == ESP_ERR_TIMEOUT)
129 : 1 : return {UsResult::TIMEOUT, 0.0f};
130 [ + + ]: 14 : if (ret != ESP_OK)
131 : 1 : return {UsResult::HW_FAULT, 0.0f};
132 : :
133 : : // 6. Measure the HIGH pulse duration
134 : 13 : uint32_t duration_us = 0;
135 : 13 : ret = measure_pulse(cfg.timeout_us, duration_us);
136 [ + + ]: 13 : if (ret == ESP_ERR_TIMEOUT)
137 : 1 : return {UsResult::TIMEOUT, 0.0f};
138 [ + + ]: 12 : if (ret != ESP_OK)
139 : 1 : return {UsResult::HW_FAULT, 0.0f};
140 : :
141 : : // 7. Convert to distance
142 : 11 : float cm = (duration_us * SOUND_SPEED_CM_PER_US) / 2.0f;
143 : :
144 [ + + + + ]: 11 : if (cm < cfg.min_distance_cm || cm > cfg.max_distance_cm) {
145 : 2 : ESP_LOGD(TAG, "Out of range: %.1f cm (limits: %.1f-%.1f)", cm, cfg.min_distance_cm, cfg.max_distance_cm);
146 : 2 : return {UsResult::OUT_OF_RANGE, 0.0f};
147 : : }
148 : :
149 : : // Inter-ping delay: applied after every ping so the sensor can reset.
150 : : // The orchestrator does not need to know about timing.
151 [ + + ]: 9 : if (cfg.ping_interval_ms > 0) {
152 : 8 : timer_hal_->delay_ms(cfg.ping_interval_ms);
153 : : }
154 : :
155 : 9 : return {UsResult::OK, cm};
156 : : }
157 : :
158 : 19 : bool UsDriver::is_echo_stuck()
159 : : {
160 : 19 : bool level = false;
161 [ + - ]: 19 : if (gpio_hal_->get_level(echo_pin_, level) != ESP_OK)
162 : : return false; // HAL error will be caught in trigger phase
163 : 19 : return level;
164 : : }
165 : :
166 : 18 : esp_err_t UsDriver::trigger(uint16_t pulse_duration_us)
167 : : {
168 : 18 : esp_err_t ret;
169 : :
170 : 18 : ret = gpio_hal_->set_level(trig_pin_, 1);
171 [ + + ]: 18 : if (ret != ESP_OK)
172 : : return ret;
173 : :
174 : 17 : ret = timer_hal_->delay_us(pulse_duration_us);
175 [ + + ]: 17 : if (ret != ESP_OK)
176 : : return ret;
177 : :
178 : 16 : ret = gpio_hal_->set_level(trig_pin_, 0);
179 : 16 : return ret;
180 : : }
181 : :
182 : 15 : esp_err_t UsDriver::wait_rising_edge(uint32_t timeout_us)
183 : : {
184 : 15 : uint64_t start = timer_hal_->get_now_us();
185 : 15 : bool level = false;
186 : :
187 : 19 : do {
188 : 19 : esp_err_t ret = gpio_hal_->get_level(echo_pin_, level);
189 [ + + ]: 19 : if (ret != ESP_OK)
190 : : return ret;
191 : :
192 [ + + ]: 18 : if (timer_hal_->get_now_us() - start > timeout_us)
193 : : return ESP_ERR_TIMEOUT;
194 [ + + ]: 17 : } while (!level);
195 : :
196 : : return ESP_OK;
197 : : }
198 : :
199 : 13 : esp_err_t UsDriver::measure_pulse(uint32_t timeout_us, uint32_t &duration_us)
200 : : {
201 : 13 : uint64_t echo_start = timer_hal_->get_now_us();
202 : 13 : bool level = true;
203 : :
204 [ + + ]: 41 : do {
205 : 43 : esp_err_t ret = gpio_hal_->get_level(echo_pin_, level);
206 [ + + ]: 43 : if (ret != ESP_OK)
207 : : return ret;
208 : :
209 [ + + ]: 42 : if (timer_hal_->get_now_us() - echo_start > timeout_us)
210 : : return ESP_ERR_TIMEOUT;
211 : : } while (level);
212 : :
213 : 11 : uint64_t echo_end = timer_hal_->get_now_us();
214 : 11 : duration_us = static_cast<uint32_t>(echo_end - echo_start);
215 : 11 : return ESP_OK;
216 : : }
|
