Maximum distances as modbus registers

[openparking.git] / firmware / firmware.c

diff --git a/firmware/firmware.c b/firmware/firmware.c
index 2019402b56c834bdeabf27899f311abe92615924..f890c15c9608429926a555adf0709d9bf2134090 100644 (file)
--- a/firmware/firmware.c
+++ b/firmware/firmware.c
@@ -2,9 +2,12 @@
 #include <avr/interrupt.h>
 #include <util/delay.h>
 #include <stdio.h>
+#include "clock.h"
 #include "modbus.h"
 
-#define TIMEOUT 0x2FF
+#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
@@ -21,6 +24,7 @@
 #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 max_distances  (hold_regs+MB_N_HOLD_REGS_EEPROM+21)
 
 static void pull_trigger(uint8_t trig)
 {
@@ -48,13 +52,13 @@ static void do_measurement(unsigned char trig)
 
        pull_trigger(trig);
 
-       starttime = TCNT1;
+       starttime = get_clock();
 
        while (to_start || to_measure) {
                uint8_t bits = 0;
-               uint16_t now = TCNT1;
+               uint16_t now = get_clock();
 
-               if (now-starttime >= TIMEOUT)
+               if (now-starttime >= ECHO_TIMEOUT)
                        break;
 
                bits = get_pin(trig);
@@ -68,10 +72,26 @@ 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 (new_d > max_distances[idx])
+                                       max_distances[idx] = new_d;
+
+                               if (old_d == 0
+                                       || old_d == -1) {
+                                       distances[idx] = new_d;
+                               } else {
+                                       distances[idx] = (
+                                               (old_d << MEASUREMENT_SHIFT)
+                                               + new_d
+                                               - old_d
+                                               ) >> MEASUREMENT_SHIFT;
+                               }
                        }
                }
        }
@@ -83,37 +103,66 @@ static void do_measurement(unsigned char trig)
                        distances[trig*N_TRIG_SENSORS + i] = 0;
 }
 
-static void do_measurements()
-{
-       uint8_t trig;
-
-       for (trig = 0; trig < N_TRIGGERS; trig++) {
-               do_measurement(trig);
-               _delay_ms(200);
-       }
-}
-
 static void led_set(uint8_t led, uint8_t state)
 {
        if (led == 0) {
-               if (state) {
-                       PORTC |= _BV(PC5);
-                       led_bitmap |= 1;
-               } else {
-                       PORTC &= ~_BV(PC5);
+               switch (state) {
+               case 0:
                        led_bitmap &= ~1;
+                       led_bitmap &= ~2;
+                       break;
+               case 1:
+                       led_bitmap |= 1;
+                       led_bitmap &= ~2;
+                       break;
+               default: // error
+                       led_bitmap |= 2;
+                       break;
                }
        } else {
-               if (state) {
-                       PORTB |= _BV(PB5);
-                       led_bitmap |= 2;
-               } else {
-                       PORTB &= ~_BV(PB5);
-                       led_bitmap &= ~2;
+               switch (state) {
+               case 0:
+                       led_bitmap &= ~4;
+                       led_bitmap &= ~8;
+                       break;
+               case 1:
+                       led_bitmap |= 4;
+                       led_bitmap &= ~8;
+                       break;
+               default:
+                       led_bitmap |= 8;
+                       break;
                }
        }
 }
 
+static void leds_update()
+{
+       if (led_bitmap & 1) {
+               PORTC |= _BV(PC5);
+       } else {
+               PORTC &= ~_BV(PC5);
+       }
+
+       if (led_bitmap & 2) {
+               DDRC &= ~_BV(PC5);
+       } else {
+               DDRC |= _BV(PC5);
+       }
+
+       if (led_bitmap & 4) {
+               PORTB |= _BV(PB5);
+       } else {
+               PORTB &= ~_BV(PB5);
+       }
+
+       if (led_bitmap & 8) {
+               DDRB |= _BV(PB5);
+       } else {
+               DDRB &= ~_BV(PB5);
+       }
+}
+
 static void eval_bitmaps()
 {
        uint16_t free_b = 0, err_b = 0, mask;
@@ -135,7 +184,9 @@ static void eval_bitmaps()
        err_bitmap  = err_b;
 
        if (led1_sensors) {
-               if (led1_sensors & free_bitmap) {
+               if (led1_sensors & err_bitmap) {
+                       led_set(0, 2);
+               } else if (led1_sensors & free_bitmap) {
                        led_set(0, 1);
                } else {
                        led_set(0, 0);
@@ -143,7 +194,9 @@ static void eval_bitmaps()
        }
 
        if (led2_sensors) {
-               if (led2_sensors & free_bitmap) {
+               if (led2_sensors & err_bitmap) {
+                       led_set(1, 2);
+               } else if (led2_sensors & free_bitmap) {
                        led_set(1, 1);
                } else {
                        led_set(1, 0);
@@ -180,12 +233,13 @@ int main()
                for (trig = 0; trig < N_TRIGGERS; trig++) {
                        uint16_t now;
                        do_measurement(trig);
-                       now = TCNT1;
-                       while (TCNT1-now < 200)
+                       now = get_clock();
+                       while (get_clock()-now < MEASUREMENT_WAIT)
                                modbus_poll();
                }
 
                eval_bitmaps();
+               leds_update(); // might be written from modbus
 //             led_set(0,
 //                     distances[4] > 100 || distances[11] > 100);
        }