Infinite reads handling

[openparking.git] / firmware / firmware.c

diff --git a/firmware/firmware.c b/firmware/firmware.c
index f890c15c9608429926a555adf0709d9bf2134090..7fbcdf18601b65f661edfb46b9cced58d937a72a 100644 (file)
--- a/firmware/firmware.c
+++ b/firmware/firmware.c
@@ -7,7 +7,7 @@
 
 #define ECHO_TIMEOUT           (CLOCK_HZ/10)   // 100 ms
 #define MEASUREMENT_WAIT       (2*ECHO_TIMEOUT)
-#define MEASUREMENT_SHIFT      2               // running avg (1 << M_SHIFT)
+#define MEASUREMENT_SHIFT      0               // running avg (1 << M_SHIFT)
 
 #define N_TRIGGERS 3
 #define N_SENSORS 12
@@ -19,12 +19,16 @@
 #define thresholds     (hold_regs+1)
 #define led1_sensors   (hold_regs[13])
 #define led2_sensors   (hold_regs[14])
+#define long_as_free   (hold_regs[15])
+#define long_thr       (hold_regs[16])
 
 #define led_bitmap     (hold_regs[MB_N_HOLD_REGS_EEPROM])
 #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 long_bitmap    (hold_regs[MB_N_HOLD_REGS_EEPROM+15])
 #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)
 {
@@ -49,6 +53,7 @@ static void do_measurement(unsigned char 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);
 
@@ -56,7 +61,7 @@ static void do_measurement(unsigned char trig)
 
        while (to_start || to_measure) {
                uint8_t bits = 0;
-               uint16_t now = get_clock();
+               now = get_clock();
 
                if (now-starttime >= ECHO_TIMEOUT)
                        break;
@@ -72,16 +77,20 @@ 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;
+#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];
-                               old_d = distances[idx];
-
                                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 == -1) {
                                        distances[idx] = new_d;
@@ -92,15 +101,40 @@ static void do_measurement(unsigned char trig)
                                                - 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 (distances[off] != -1 && 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
+                        * and notify the state in the bitmap.
+                        */
+                       distances[off] = now - starttimes[i];
+               }
+       }
 }
 
 static void led_set(uint8_t led, uint8_t state)
@@ -157,31 +191,39 @@ static void leds_update()
        }
 
        if (led_bitmap & 8) {
-               DDRB |= _BV(PB5);
-       } else {
                DDRB &= ~_BV(PB5);
+       } else {
+               DDRB |= _BV(PB5);
        }
 }
 
 static void eval_bitmaps()
 {
-       uint16_t free_b = 0, err_b = 0, mask;
+       uint16_t free_b = 0, err_b = 0, long_b = 0, mask;
        uint8_t i;
 
        for (i = 0; i < N_SENSORS; i++) {
                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;
+                               if (long_thr && distances[i] > long_thr) {
+                                       long_b |= mask;
+                                       if (long_as_free & mask) {
+                                               free_b |= mask;
+                                       }
+                               } else {
+                                       free_b |= mask;
+                               }
                        }
                }
        }
 
        free_bitmap = free_b;
        err_bitmap  = err_b;
+       long_bitmap = long_b;
 
        if (led1_sensors) {
                if (led1_sensors & err_bitmap) {