Working modbus

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);
        }
 }
 

diff --git a/firmware/modbus.c b/firmware/modbus.c
index 937d9d90b0f543775c81cd8507729311a7eff57b..a49bd877e1c778357e5f3f39f3f1b5ece6defbec 100755 (executable)
--- a/firmware/modbus.c
+++ b/firmware/modbus.c
@@ -5,11 +5,14 @@
  * All bugs by Jan "Yenya" Kasprzak <kas@fi.muni.cz> :-)
  */
 
+#include <avr/eeprom.h>
 #include <avr/io.h>
 #include <avr/interrupt.h>
 #include <util/atomic.h>
 #include <util/delay.h>
 
+#include "modbus.h"
+
 #define BUFSIZE 128    // must be a power of two
 
 // configure the control pin
@@ -33,7 +36,9 @@ typedef uint8_t  bufptr_t;
 static volatile bufptr_t rx_bytes, tx_head, tx_tail;
 static volatile uint8_t rxbuf[BUFSIZE], txbuf[BUFSIZE];
 static volatile uint16_t last_rx;
-static volatile uint8_t unit_id;
+#ifndef mb_unit_id
+static uint8_t mb_unit_id;
+#endif
 
 #define UART_BAUD      9600
 #define UBRR_VAL        ((F_CPU + 8UL * UART_BAUD) / (16UL*UART_BAUD) - 1)
@@ -47,12 +52,32 @@ static volatile uint8_t unit_id;
  */
 #define TIMEOUT                (28*CLOCK_SPEED/UART_BAUD)
 
-void modbus_init()
+uint16_t hold_regs[MB_N_HOLD_REGS];
+
+#if MB_N_HOLD_REGS_EEPROM > 0
+static uint16_t hold_regs_ee[MB_N_HOLD_REGS_EEPROM] EEMEM = {
+       42, 
+       0, 0, 0, 30, 30, 30, 30, 0, 0, 0, 0, 30,
+       (1 << 4) | (1 << 11), // LED 1
+       0, // LED 2
+};
+
+#endif
+
+void modbus_init(uint8_t unit)
 {
        rx_bytes = 0;
        tx_head = tx_tail = 0;
 
-       unit_id = 42;
+       if (unit)
+               mb_unit_id = unit;
+#if MB_N_HOLD_REGS_EEPROM > 0
+       do {
+               int i;
+               for (i = 0; i < MB_N_HOLD_REGS_EEPROM; i++)
+                       hold_regs[i] = eeprom_read_word(&hold_regs_ee[i]);
+       } while (0);
+#endif
 
        ctl_pin_off();
        ctl_pin_setup();
@@ -103,17 +128,69 @@ static uint16_t compute_crc(volatile uint8_t *buf, bufptr_t len)
        return crc;
 }
 
-static void make_exception(uint8_t func, uint8_t code)
+static void make_exception(mb_exception code)
+{
+       txbuf[1] |= 0x80;
+       txbuf[2] = code;
+       tx_head = 3;
+}
+
+#define get_word(ptr, off) (((uint16_t)ptr[off] << 8) | ptr[off+1])
+void put_byte(uint8_t byte)
+{
+       txbuf[tx_head++] = byte;
+}
+
+void put_word(uint16_t word)
 {
-       txbuf[tx_head++] = unit_id;
-       txbuf[tx_head++] = func | 0x80;
-       txbuf[tx_head++] = code;
+       txbuf[tx_head++] = word >> 8;
+       txbuf[tx_head++] = word & 0xFF;
+}
+
+static mb_exception read_holding_regs(uint16_t start, uint16_t len)
+{
+       if (len > BUFSIZE/2 - 3)
+               return MB_ILLEGAL_ADDR;
+
+       if (start < MB_HOLD_REGS_BASE
+               || start + len > MB_HOLD_REGS_BASE + MB_N_HOLD_REGS)
+               return MB_ILLEGAL_ADDR;
+
+       put_byte(2*len);
+
+       start -= MB_HOLD_REGS_BASE;
+       while(len--)
+               put_word(hold_regs[start++]);
+
+       return MB_OK;
+}
+
+static mb_exception write_single_reg(uint16_t reg, uint16_t val)
+{
+       if (reg < MB_HOLD_REGS_BASE
+               || reg >= MB_HOLD_REGS_BASE + MB_N_HOLD_REGS)
+               return MB_ILLEGAL_ADDR;
+
+       if (!hold_reg_is_valid(reg, val))
+               return MB_ILLEGAL_VAL;
+
+       reg -= MB_HOLD_REGS_BASE;
+       hold_regs[reg] = val;
+#if MB_N_HOLD_REGS_EEPROM > 0
+       if (reg < MB_N_HOLD_REGS_EEPROM)
+               eeprom_write_word(&hold_regs_ee[reg], val);
+#endif
+       put_word(reg + MB_HOLD_REGS_BASE);
+       put_word(val);
+
+       return MB_OK;
 }
 
 void modbus_poll()
 {
        bufptr_t packet_len;
        uint16_t crc;
+       uint8_t rv;
 
        ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
                if (rx_bytes == 0) // nothing received yet
@@ -127,7 +204,7 @@ void modbus_poll()
                        return;
                }
 
-               if (rxbuf[0] != unit_id) { // not for myself
+               if (rxbuf[0] != mb_unit_id) { // not for myself
                        rx_bytes = 0;
                        return;
                }
@@ -146,13 +223,24 @@ void modbus_poll()
                || (crc >> 8) != rxbuf[packet_len-1]) // bad crc
                goto out;
 
-       tx_head = 0;
+       txbuf[0] = rxbuf[0]; // not mb_unit_id in case it gets changed
+       txbuf[1] = rxbuf[1];
+       tx_head = 2;
 
+       rv = MB_OK;
        switch (rxbuf[1]) { // function
+       case 3:
+               rv = read_holding_regs(get_word(rxbuf, 2), get_word(rxbuf, 4));
+               break;
+       case 6:
+               rv = write_single_reg(get_word(rxbuf, 2), get_word(rxbuf, 4));
+               break;
        default:
-               make_exception(rxbuf[1], 1); // illegal function
+               make_exception(MB_ILLEGAL_FUNC); // illegal function
        }
-
+       
+       if (rv)
+               make_exception(rv);
 send:
        if (tx_head) {
                crc = compute_crc(txbuf, tx_head);

diff --git a/firmware/modbus.h b/firmware/modbus.h
index 4218317c54f627dae421c996f57e04b4c73d2cc1..937b8e8d71adb618581cd06f10216583ad281f0d 100755 (executable)
--- a/firmware/modbus.h
+++ b/firmware/modbus.h
@@ -8,7 +8,22 @@
  * All bugs by Jan "Yenya" Kasprzak <kas@fi.muni.cz> :-)
  */
 
-void modbus_init();
+void modbus_init(uint8_t unit_id);
 void modbus_poll();
 
+typedef enum {
+       MB_OK = 0,
+       MB_ILLEGAL_FUNC = 1,
+       MB_ILLEGAL_ADDR = 2,
+       MB_ILLEGAL_VAL  = 3,
+} mb_exception;
+
+#define MB_HOLD_REGS_BASE      1000
+#define MB_N_HOLD_REGS         40
+#define MB_N_HOLD_REGS_EEPROM  20
+extern uint16_t hold_regs[MB_N_HOLD_REGS];
+#define mb_unit_id     (hold_regs[0])
+
+uint8_t hold_reg_is_valid(uint16_t reg, uint16_t val);
+
 #endif /* MODBUS_H__ */