Firmware SDK
twr_hc_sr04.c
1 #include <twr_hc_sr04.h>
2 #include <twr_system.h>
3 #include <stm32l0xx.h>
4 #include <twr_timer.h>
5 
6 // Timer resolution in microseconds
7 #define _TWR_HC_SR04_RESOLUTION 5
8 
9 static void _twr_hc_sr04_task_interval(void *param);
10 static void _twr_hc_sr04_task_notify(void *param);
11 static void _twr_hc_sr04_tim2_iqr_handler(void *param);
12 static void _twr_hc_sr04_tim3_iqr_handler(void *param);
13 
15 {
16  twr_hc_sr04_init(self, TWR_GPIO_P4, TWR_HC_SR04_ECHO_P5);
17 }
18 
19 void twr_hc_sr04_init(twr_hc_sr04_t *self, twr_gpio_channel_t trig, twr_hc_sr04_echo_t echo)
20 {
21  memset(self, 0, sizeof(*self));
22 
23  self->_echo = echo;
24  self->_trig = trig;
25 
26  self->_task_id_interval = twr_scheduler_register(_twr_hc_sr04_task_interval, self, TWR_TICK_INFINITY);
27 
28  // Pin Trig
29  twr_gpio_init(self->_trig);
30 
32 
33  twr_gpio_set_output(self->_trig, 0);
34 
35  // Pin Echo
36  if (self->_echo == TWR_HC_SR04_ECHO_P5)
37  {
39 
41 
42  // Enable TIM2 clock
43  RCC->APB1ENR |= RCC_APB1ENR_TIM2EN;
44 
45  // Errata workaround
46  RCC->APB1ENR;
47 
48  // Capture 1 and Capture 2 is connected to TI1
49  TIM2->CCMR1 |= TIM_CCMR1_CC2S_1 | TIM_CCMR1_CC1S_0;
50 
51  // Capture 2 is sensitive on falling edge
52  TIM2->CCER |= 1 << TIM_CCER_CC2P_Pos;
53 
54  // Set prescaler to 5 * 32 (5 microseconds resolution)
55  TIM2->PSC = _TWR_HC_SR04_RESOLUTION * 32 - 1;
56 
57  twr_timer_set_irq_handler(TIM2, _twr_hc_sr04_tim2_iqr_handler, self);
58 
59  // Enable TIM2 interrupts
60  NVIC_EnableIRQ(TIM2_IRQn);
61  }
62  else if (self->_echo == TWR_HC_SR04_ECHO_P8)
63  {
65 
67 
68  // Enable TIM3 clock
69  RCC->APB1ENR |= RCC_APB1ENR_TIM3EN;
70 
71  // Errata workaround
72  RCC->APB1ENR;
73 
74  // Capture 4 and Capture 3 is connected to TI3
75  TIM3->CCMR2 |= TIM_CCMR2_CC4S_1 | TIM_CCMR2_CC3S_0;
76 
77  // Capture 4 is sensitive on falling edge
78  TIM3->CCER |= 1 << TIM_CCER_CC4P_Pos;
79 
80  // Set prescaler to 5 * 32 (5 microseconds resolution)
81  TIM3->PSC = _TWR_HC_SR04_RESOLUTION * 32 - 1;
82 
83  twr_timer_set_irq_handler(TIM3, _twr_hc_sr04_tim3_iqr_handler, self);
84 
85  // Enable TIM3 interrupts
86  NVIC_EnableIRQ(TIM3_IRQn);
87  }
88 
90 }
91 
92 void twr_hc_sr04_set_event_handler(twr_hc_sr04_t *self, void (*event_handler)(twr_hc_sr04_t *, twr_hc_sr04_event_t, void *), void *event_param)
93 {
94  self->_event_handler = event_handler;
95  self->_event_param = event_param;
96 }
97 
99 {
100  self->_update_interval = interval;
101 
102  if (self->_update_interval == TWR_TICK_INFINITY)
103  {
104  twr_scheduler_plan_absolute(self->_task_id_interval, TWR_TICK_INFINITY);
105  }
106  else
107  {
108  twr_scheduler_plan_relative(self->_task_id_interval, self->_update_interval);
109 
110  twr_hc_sr04_measure(self);
111  }
112 }
113 
115 {
116  if (self->_measurement_active)
117  {
118  return false;
119  }
120 
121  // Enable PLL
122  twr_system_pll_enable();
123 
124  self->_measurement_active = true;
125 
126  self->_measurement_valid = false;
127 
128  self->_task_id_notify = twr_scheduler_register(_twr_hc_sr04_task_notify, self, TWR_TICK_INFINITY);
129 
130  if (self->_echo == TWR_HC_SR04_ECHO_P5)
131  {
132  // Set timeout register (250 milliseconds)
133  TIM2->CCR3 = 250000 / _TWR_HC_SR04_RESOLUTION;
134 
135  // Trigger update
136  TIM2->EGR = TIM_EGR_UG;
137 
138  // Clear Capture 1 / Capture 2 overcapture flags
139  TIM2->SR &= ~(TIM_SR_CC1OF | TIM_SR_CC2OF);
140 
141  // Clear Capture 1 / Capture 2 / Compare 3 interrupt flags
142  TIM2->SR &= ~(TIM_SR_CC1IF | TIM_SR_CC2IF | TIM_SR_CC3IF);
143 
144  // Enable timer counter
145  TIM2->CR1 |= TIM_CR1_CEN;
146 
147  // Enable Capture 1 / Capture 2
148  TIM2->CCER |= TIM_CCER_CC1E | TIM_CCER_CC2E;
149 
150  // Enable Capture 2 / Compare 3 interrupt
151  TIM2->DIER |= TIM_DIER_CC2IE | TIM_DIER_CC3IE;
152  }
153  else if (self->_echo == TWR_HC_SR04_ECHO_P8)
154  {
155  // Set timeout register (250 milliseconds)
156  TIM3->CCR1 = 250000 / _TWR_HC_SR04_RESOLUTION;
157 
158  // Trigger update
159  TIM3->EGR = TIM_EGR_UG;
160 
161  // Clear Capture 4 / Capture 3 overcapture flags
162  TIM3->SR &= ~(TIM_SR_CC4OF | TIM_SR_CC3OF);
163 
164  // Clear Capture 4 / Capture 3 / Compare 1 interrupt flags
165  TIM3->SR &= ~(TIM_SR_CC4IF | TIM_SR_CC3IF | TIM_SR_CC1IF);
166 
167  // Enable timer counter
168  TIM3->CR1 |= TIM_CR1_CEN;
169 
170  // Enable Capture 4 / Capture 3
171  TIM3->CCER |= TIM_CCER_CC4E | TIM_CCER_CC3E;
172 
173  // Enable Capture 4 / Compare 1 interrupt
174  TIM3->DIER |= TIM_DIER_CC4IE | TIM_DIER_CC1IE;
175  }
176 
177  twr_gpio_set_output(self->_trig, 1);
178 
179  twr_timer_start();
180 
181  twr_timer_delay(10);
182 
183  twr_timer_stop();
184 
185  twr_gpio_set_output(self->_trig, 0);
186 
187  return true;
188 }
189 
191 {
192  if (!self->_measurement_valid)
193  {
194  *millimeter = NAN;
195 
196  return false;
197  }
198 
199  *millimeter = _TWR_HC_SR04_RESOLUTION * (float) self->_echo_duration / 5.8;
200 
201  return true;
202 }
203 
204 static void _twr_hc_sr04_task_interval(void *param)
205 {
206  twr_hc_sr04_t *self = (twr_hc_sr04_t *) param;
207 
208  twr_hc_sr04_measure(self);
209 
210  twr_scheduler_plan_current_relative(self->_update_interval);
211 }
212 
213 static void _twr_hc_sr04_task_notify(void *param)
214 {
215  twr_hc_sr04_t *self = (twr_hc_sr04_t *) param;
216 
217  // Disable PLL
218  twr_system_pll_disable();
219 
220  self->_measurement_active = false;
221 
222  twr_scheduler_unregister(self->_task_id_notify);
223 
224  if (!self->_measurement_valid)
225  {
226  if (self->_event_handler != NULL)
227  {
228  self->_event_handler(self, TWR_HC_SR04_EVENT_ERROR, self->_event_param);
229  }
230  }
231  else if (self->_event_handler != NULL)
232  {
233  self->_event_handler(self, TWR_HC_SR04_EVENT_UPDATE, self->_event_param);
234  }
235 }
236 
237 static void _twr_hc_sr04_tim3_iqr_handler(void *param)
238 {
239  twr_hc_sr04_t *self = (twr_hc_sr04_t *) param;
240 
241  // Disable Capture 4 / Compare 1 interrupt
242  TIM3->DIER &= ~(TIM_DIER_CC4IE | TIM_DIER_CC1IE);
243 
244  // Disable Capture 4 / Capture 3
245  TIM3->CCER &= ~(TIM_CCER_CC4E | TIM_CCER_CC3E);
246 
247  // Disable timer counter
248  TIM3->CR1 &= ~TIM_CR1_CEN;
249 
250  // If not Compare 1 interrupt... (timeout)
251  if ((TIM3->SR & TIM_SR_CC1IF) == 0)
252  {
253  // If not Capture 4 / Capture 3 overcapture...
254  if ((TIM3->SR & TIM_SR_CC4OF) == 0 && (TIM3->SR & TIM_SR_CC3OF) == 0)
255  {
256  // If Capture 4 interrupt... (falling edge)
257  if ((TIM3->SR & TIM_SR_CC4IF) != 0)
258  {
259  // Retrieve falling edge capture mark
260  uint16_t falling_capture = TIM3->CCR4;
261 
262  // If Capture 3 interrupt... (rising edge)
263  if ((TIM3->SR & TIM_SR_CC3IF) != 0)
264  {
265  // Retrieve rising edge capture mark
266  uint16_t rising_capture = TIM3->CCR3;
267 
268  // Calculate echo duration (distance between rising and falling edge)
269  self->_echo_duration = falling_capture - rising_capture;
270 
271  if (self->_echo_duration <= 30000 / _TWR_HC_SR04_RESOLUTION)
272  {
273  // Indicate success
274  self->_measurement_valid = true;
275  }
276  }
277  }
278  }
279  }
280 
281  // Schedule task for immediate execution
282  twr_scheduler_plan_now(self->_task_id_notify);
283 }
284 
285 static void _twr_hc_sr04_tim2_iqr_handler(void *param)
286 {
287  twr_hc_sr04_t *self = (twr_hc_sr04_t *) param;
288 
289  // Disable Capture 2 / Compare 3 interrupt
290  TIM2->DIER &= ~(TIM_DIER_CC2IE | TIM_DIER_CC3IE);
291 
292  // Disable Capture 1 / Capture 2
293  TIM2->CCER &= ~(TIM_CCER_CC1E | TIM_CCER_CC2E);
294 
295  // Disable timer counter
296  TIM2->CR1 &= ~TIM_CR1_CEN;
297 
298  // If not Compare 3 interrupt... (timeout)
299  if ((TIM2->SR & TIM_SR_CC3IF) == 0)
300  {
301  // If not Capture 1 / Capture 2 overcapture...
302  if ((TIM2->SR & TIM_SR_CC1OF) == 0 && (TIM2->SR & TIM_SR_CC2OF) == 0)
303  {
304  // If Capture 2 interrupt... (falling edge)
305  if ((TIM2->SR & TIM_SR_CC2IF) != 0)
306  {
307  // Retrieve falling edge capture mark
308  uint16_t falling_capture = TIM2->CCR2;
309 
310  // If Capture 3 interrupt... (rising edge)
311  if ((TIM2->SR & TIM_SR_CC1IF) != 0)
312  {
313  // Retrieve rising edge capture mark
314  uint16_t rising_capture = TIM2->CCR1;
315 
316  // Calculate echo duration (distance between rising and falling edge)
317  self->_echo_duration = falling_capture - rising_capture;
318 
319  if (self->_echo_duration <= 30000 / _TWR_HC_SR04_RESOLUTION)
320  {
321  // Indicate success
322  self->_measurement_valid = true;
323  }
324  }
325  }
326  }
327  }
328 
329  // Schedule task for immediate execution
330  twr_scheduler_plan_now(self->_task_id_notify);
331 }
void twr_gpio_set_mode(twr_gpio_channel_t channel, twr_gpio_mode_t mode)
Set mode of operation for GPIO channel.
Definition: twr_gpio.c:367
void twr_scheduler_plan_relative(twr_scheduler_task_id_t task_id, twr_tick_t tick)
Schedule specified task to tick relative from current spin.
void twr_hc_sr04_init_sensor_module(twr_hc_sr04_t *self)
Initialize HC-SR04 for sensor module.
Definition: twr_hc_sr04.c:14
void twr_scheduler_plan_current_relative(twr_tick_t tick)
Schedule current task to tick relative from current spin.
GPIO channel operates as output.
Definition: twr_gpio.h:108
twr_scheduler_task_id_t twr_scheduler_register(void(*task)(void *), void *param, twr_tick_t tick)
Register task in scheduler.
Definition: twr_scheduler.c:53
void twr_hc_sr04_set_event_handler(twr_hc_sr04_t *self, void(*event_handler)(twr_hc_sr04_t *, twr_hc_sr04_event_t, void *), void *event_param)
Set callback function.
Definition: twr_hc_sr04.c:92
GPIO channel operates in alternate mode AF5.
Definition: twr_gpio.h:135
GPIO channel P8.
Definition: twr_gpio.h:39
void twr_timer_stop(void)
Stop timer.
Definition: twr_timer.c:42
struct twr_hc_sr04_t twr_hc_sr04_t
HC-SR04 instance.
Definition: twr_hc_sr04.h:32
twr_hc_sr04_event_t
Definition: twr_hc_sr04.h:12
void twr_gpio_init(twr_gpio_channel_t channel)
Initialize GPIO channel.
Definition: twr_gpio.c:325
bool twr_hc_sr04_measure(twr_hc_sr04_t *self)
Start measurement manually.
Definition: twr_hc_sr04.c:114
twr_gpio_channel_t
GPIO channels.
Definition: twr_gpio.h:12
bool twr_timer_set_irq_handler(TIM_TypeDef *tim, void(*irq_handler)(void *), void *irq_param)
Register timer IRQ handler.
Definition: twr_timer.c:86
uint64_t twr_tick_t
Timestamp data type.
Definition: twr_tick.h:16
void twr_timer_init(void)
Initialize timer.
Definition: twr_timer.c:23
void twr_hc_sr04_init(twr_hc_sr04_t *self, twr_gpio_channel_t trig, twr_hc_sr04_echo_t echo)
Initialize HC-SR04.
Definition: twr_hc_sr04.c:19
GPIO channel operates in alternate mode AF2.
Definition: twr_gpio.h:126
GPIO channel P4, A4, DAC0.
Definition: twr_gpio.h:27
void twr_gpio_set_output(twr_gpio_channel_t channel, int state)
Set output state for GPIO channel.
Definition: twr_gpio.c:471
void twr_scheduler_unregister(twr_scheduler_task_id_t task_id)
Unregister specified task.
Definition: twr_scheduler.c:77
void twr_hc_sr04_set_update_interval(twr_hc_sr04_t *self, twr_tick_t interval)
Set measurement interval.
Definition: twr_hc_sr04.c:98
bool twr_hc_sr04_get_distance_millimeter(twr_hc_sr04_t *self, float *millimeter)
Get measured distance in millimeters.
Definition: twr_hc_sr04.c:190
GPIO channel P5, A5, DAC1.
Definition: twr_gpio.h:30
#define TWR_TICK_INFINITY
Maximum timestamp value.
Definition: twr_tick.h:12
void twr_scheduler_plan_absolute(twr_scheduler_task_id_t task_id, twr_tick_t tick)
Schedule specified task to absolute tick.
void twr_timer_delay(uint16_t microseconds)
Relative delay.
Definition: twr_timer.c:59
void twr_scheduler_plan_now(twr_scheduler_task_id_t task_id)
Schedule specified task for immediate execution.
void twr_timer_start(void)
Start timer.
Definition: twr_timer.c:28