Working modbus

[openparking.git] / firmware / firmware.c

diff --git a/firmware/firmware.c b/firmware/firmware.c
index 9b925210d48cbceee072333ea204e34a77cbfaab..ce4a91486cda276c43235dca59fd77b373af6bd3 100755 (executable)
--- a/firmware/firmware.c
+++ b/firmware/firmware.c
@@ -10,7 +10,17 @@
 #define N_SENSORS 12
 #define N_TRIG_SENSORS 4
 
-static int16_t distances[N_SENSORS];
+// static int16_t distances[N_SENSORS];
+
+// hold_regs[0] is unit ID
+#define thresholds     (hold_regs+1)
+#define led1_sensors   (hold_regs[13])
+#define led2_sensors   (hold_regs[14])
+
+#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])
 
 static void pull_trigger(uint8_t trig)
 {
@@ -89,22 +99,71 @@ static void led_set(uint8_t led, uint8_t state)
                if (state) {
                        PORTD |= _BV(PD4);
                        // PORTC |= _BV(PC5);
+                       led_bitmap |= 1;
                } else {
                        PORTD &= ~_BV(PD4);
                        // PORTC &= ~_BV(PC5);
+                       led_bitmap &= ~1;
                }
        } else {
                if (state) {
                        PORTB |= _BV(PB5);
+                       led_bitmap |= 2;
                } else {
                        PORTB &= ~_BV(PB5);
+                       led_bitmap &= ~2;
                }
        }
 }
 
+static void eval_bitmaps()
+{
+       uint16_t free_b = 0, err_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) {
+                               err_b |= mask;
+                       } else if (distances[i] > thresholds[i]) {
+                               free_b |= mask;
+                       }
+               }
+       }
+
+       free_bitmap = free_b;
+       err_bitmap  = err_b;
+
+       if (led1_sensors) {
+               if (led1_sensors & free_bitmap) {
+                       led_set(0, 1);
+               } else {
+                       led_set(0, 0);
+               }
+       }
+
+       if (led2_sensors) {
+               if (led2_sensors & free_bitmap) {
+                       led_set(1, 1);
+               } else {
+                       led_set(1, 0);
+               }
+       }
+}
+
+uint8_t hold_reg_is_valid(uint16_t reg, uint16_t val)
+{
+        if (reg == MB_HOLD_REGS_BASE) 
+                return val > 0 && val <= 247;
+
+        return 1;
+}
+
 int main()
 {
-       modbus_init();
+       modbus_init(0);
 
        // output pins
        DDRD |= _BV(PD7); // Trig D
@@ -121,10 +180,18 @@ int main()
        sei();
 
        while(1) {
-               // do_measurements();
-               modbus_poll();
-               led_set(0,
-                       distances[4] > 100 || distances[11] > 100);
+               uint8_t trig;
+               for (trig = 0; trig < N_TRIGGERS; trig++) {
+                       uint16_t now;
+                       do_measurement(trig);
+                       now = TCNT1;
+                       while (TCNT1-now < 200)
+                               modbus_poll();
+               }
+
+               eval_bitmaps();
+//             led_set(0,
+//                     distances[4] > 100 || distances[11] > 100);
        }
 }