Timeout adjustments

[openparking.git] / firmware / firmware.c

diff --git a/firmware/firmware.c b/firmware/firmware.c
index bbba9874c0231bf0467e01c249495dc264e52b9d..fb095f5a0b12966f64f749735e639bb4bb66afd9 100644 (file)
--- a/firmware/firmware.c
+++ b/firmware/firmware.c
@@ -5,8 +5,9 @@
 #include "clock.h"
 #include "modbus.h"
 
-#define ECHO_TIMEOUT           (CLOCK_HZ/10)   // 10 ms
-#define MEASUREMENT_WAIT       (CLOCK_HZ/3)    // three triggers per second
+#define ECHO_TIMEOUT           (CLOCK_HZ/10)   // 100 ms
+#define MEASUREMENT_WAIT       (2*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;
+                               }
                        }
                }
        }