static void *colllision_detection(void *arg)
{
uint32_t lw = xtimer_now();
while (1) {
/* trigger sensor reading */
srf02_trigger(&dist_front, SRF02_MODE_REAL_CM);
srf02_trigger(&dist_back, SRF02_MODE_REAL_CM);
/* wait for results */
xtimer_usleep(SRF02_RANGE_DELAY);
/* read distance data */
front_filter[filter_pos] = srf02_read(&dist_front);
xtimer_usleep(1); /* hack, otherwise the 2nd srf02_read f***s up */
back_filter[filter_pos] = srf02_read(&dist_back);
// printf(" f: %3i, b: %3i %i\n", (int)front_filter[filter_pos], (int)back_filter[filter_pos], filter_pos);
filter_pos = (++filter_pos >= FILTER_SIZE) ? 0 : filter_pos;
/* analyze data and trigger events base on it */
uint16_t fd = 0;
uint16_t bd = 0;
for (int i = 0; i < FILTER_SIZE; i++) {
fd += front_filter[i];
bd += back_filter[i];
}
if ((fd < (FILTER_SIZE * CONF_DIST_THOLD)) && (front_blocked == 0)) {
front_blocked = 1;
event(EVT_FRONT_BLOCKED);
}
else if ((fd >= (FILTER_SIZE * CONF_DIST_THOLD)) && (front_blocked == 1)) {
front_blocked = 0;
event(EVT_FRONT_FREE);
}
if ((bd < (FILTER_SIZE * CONF_DIST_THOLD)) && (back_blocked == 0)) {
back_blocked = 1;
event(EVT_BACK_BLOCKED);
}
else if ((bd >= (FILTER_SIZE * CONF_DIST_THOLD)) && (back_blocked == 1)) {
back_blocked = 0;
event(EVT_BACK_FREE);
}
xtimer_usleep_until(&lw, CONF_DIST_SENSE_DELAY);
}
return NULL;
}
uint16_t srf02_get_distance(srf02_t *dev, srf02_mode_t mode)
{
/* trigger a new reading */
srf02_trigger(dev, mode);
/* give the sensor the required time for sampling */
xtimer_usleep(SRF02_RANGE_DELAY);
/* get the results */
return srf02_read(dev);
}
