From dc770d3e8804b9cb81fd1d0c7fc7336a14dd7c1f Mon Sep 17 00:00:00 2001 From: "Jan \"Yenya\" Kasprzak" Date: Fri, 22 May 2015 23:13:25 +0200 Subject: [PATCH] measurements: running avg --- firmware/firmware.c | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/firmware/firmware.c b/firmware/firmware.c index f77e67a..c9c4317 100644 --- a/firmware/firmware.c +++ b/firmware/firmware.c @@ -7,6 +7,7 @@ #define ECHO_TIMEOUT (CLOCK_HZ/20) // 50 ms #define MEASUREMENT_WAIT (4*ECHO_TIMEOUT) +#define MEASUREMENT_SHIFT 2 // running avg (1 << M_SHIFT) #define N_TRIGGERS 3 #define N_SENSORS 12 @@ -70,10 +71,23 @@ static void do_measurement(unsigned char trig) to_start &= ~mask; to_measure |= mask; } else if ((to_measure & mask) && !(bits & mask)) { + uint16_t old_d, new_d; + uint8_t idx = trig*N_TRIG_SENSORS+i; // echo end to_measure &= ~mask; - distances[trig*N_TRIG_SENSORS + i] - = now - starttimes[i]; + new_d = now - starttimes[i]; + old_d = distances[idx]; + + if (old_d == 0 + || old_d == -1) { + distances[idx] = new_d; + } else { + distances[idx] = ( + (old_d << MEASUREMENT_SHIFT) + + new_d + - old_d + ) >> MEASUREMENT_SHIFT; + } } } } -- 2.43.0