X-Git-Url: https://www.fi.muni.cz/~kas/git//home/kas/public_html/git/?a=blobdiff_plain;f=firmware%2Ffirmware.c;fp=firmware%2Ffirmware.c;h=c9c4317db1de3d1571498dde1ee45f975488aca4;hb=dc770d3e8804b9cb81fd1d0c7fc7336a14dd7c1f;hp=f77e67a570377132a7bcaf815c207be5ed3dad05;hpb=732666831f2b77bf96f272f184f426d0fff1ddab;p=openparking.git

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;
+				}
 			}
 		}
 	}