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