#include "clock.h"
#include "modbus.h"
-#define ECHO_TIMEOUT (CLOCK_HZ/20) // 50 ms
-#define MEASUREMENT_WAIT (4*ECHO_TIMEOUT)
-#define MEASUREMENT_SHIFT 2 // running avg (1 << M_SHIFT)
+#define ECHO_TIMEOUT (CLOCK_HZ/10) // 100 ms
+#define MEASUREMENT_WAIT (2*ECHO_TIMEOUT)
+#define MEASUREMENT_SHIFT 0 // running avg (1 << M_SHIFT)
#define N_TRIGGERS 3
#define N_SENSORS 12
#define distances (hold_regs+MB_N_HOLD_REGS_EEPROM+1)
#define free_bitmap (hold_regs[MB_N_HOLD_REGS_EEPROM+13])
#define err_bitmap (hold_regs[MB_N_HOLD_REGS_EEPROM+14])
+#define max_distances (hold_regs+MB_N_HOLD_REGS_EEPROM+21)
+#define err_counts (hold_regs+MB_N_HOLD_REGS_EEPROM+41)
static void pull_trigger(uint8_t trig)
{
uint16_t starttimes[N_TRIG_SENSORS], starttime;
uint8_t to_start = (1 << N_TRIG_SENSORS) - 1;
uint8_t to_measure = 0, i;
+ uint16_t now;
pull_trigger(trig);
while (to_start || to_measure) {
uint8_t bits = 0;
- uint16_t now = get_clock();
+ now = get_clock();
if (now-starttime >= ECHO_TIMEOUT)
break;
to_start &= ~mask;
to_measure |= mask;
} else if ((to_measure & mask) && !(bits & mask)) {
- uint16_t old_d, new_d;
+#if MEASUREMENT_SHIFT > 0
+ uint16_t old_d;
+#endif
+ uint16_t new_d;
uint8_t idx = trig*N_TRIG_SENSORS+i;
// echo end
to_measure &= ~mask;
new_d = now - starttimes[i];
+ if (new_d > max_distances[idx])
+ max_distances[idx] = new_d;
+
+#if MEASUREMENT_SHIFT > 0
old_d = distances[idx];
if (old_d == 0
- old_d
) >> MEASUREMENT_SHIFT;
}
+#else
+ distances[idx] = new_d;
+#endif
}
}
}
- 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))
- distances[trig*N_TRIG_SENSORS + i] = 0;
+ 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);
+ }
+ /*
+ * If the echo pulse is too long, do not treat it
+ * as error, just count it as maximum length.
+ */
+ distances[off] = now - starttimes[i];
+ }
+ }
}
static void led_set(uint8_t led, uint8_t state)
mask = 1 << i;
if (thresholds[i]) {
- if (distances[i] == -1 || distances[i] == 0) {
+ if (distances[i] == -1) {
err_b |= mask;
} else if (distances[i] > thresholds[i]) {
free_b |= mask;