- for (i = 0; i < N_TRIG_SENSORS; i++)
- if (to_start & (1 << i))
- distances[trig*N_TRIG_SENSORS + i] = -1;
- else if (to_measure & (1 << i))
+ for (i = 0; i < N_TRIG_SENSORS; i++) {
+ uint8_t off = trig*N_TRIG_SENSORS + i;
+
+ if (to_start & (1 << i)) { // echo not received
+ uint16_t err_count = err_counts[off] & 0xFF;
+ if (err_count < 255) {
+ err_count++;
+ err_counts[off] = (err_counts[off] & 0xFF00)
+ | err_count;
+ }
+ distances[off] = -1;
+ } else if (to_measure & (1 << i)) { // echo pulse too long
+ uint16_t err_count = err_counts[off] >> 8;
+
+ if (err_count < 255) {
+ err_count++;
+ err_counts[off] = (err_counts[off] & 0x00FF)
+ | (err_count << 8);
+ }