error counters to modbus

[openparking.git] / firmware / firmware.c

diff --git a/firmware/firmware.c b/firmware/firmware.c
index 8e973510ae55d1577ecc28b2ec2b413d85d59f43..c2dd944e54f12566a8bee73473076fe99f7b03e4 100644 (file)
--- a/firmware/firmware.c
+++ b/firmware/firmware.c
@@ -25,6 +25,7 @@
 #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)
 {
@@ -104,16 +105,32 @@ static void do_measurement(unsigned char trig)
                }
        }
 
-       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))
+       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[trig*N_TRIG_SENSORS + i]
-                               = now - starttimes[i];
+                       distances[off] = now - starttimes[i];
+               }
+       }
 }
 
 static void led_set(uint8_t led, uint8_t state)