firmware/firmware.c

   1 #include <avr/io.h>
   2 #include <avr/interrupt.h>
   3 #include <util/delay.h>
   4 #include <stdio.h>
   5 #include "clock.h"
   6 #include "modbus.h"
   7
   8 #define ECHO_TIMEOUT            (CLOCK_HZ/10)   // 100 ms
   9 #define MEASUREMENT_WAIT        (2*ECHO_TIMEOUT)
  10 #define MEASUREMENT_SHIFT       2               // running avg (1 << M_SHIFT)
  11
  12 #define N_TRIGGERS 3
  13 #define N_SENSORS 12
  14 #define N_TRIG_SENSORS 4
  15
  16 // static int16_t distances[N_SENSORS];
  17
  18 // hold_regs[0] is unit ID
  19 #define thresholds      (hold_regs+1)
  20 #define led1_sensors    (hold_regs[13])
  21 #define led2_sensors    (hold_regs[14])
  22
  23 #define led_bitmap      (hold_regs[MB_N_HOLD_REGS_EEPROM])
  24 #define distances       (hold_regs+MB_N_HOLD_REGS_EEPROM+1)
  25 #define free_bitmap     (hold_regs[MB_N_HOLD_REGS_EEPROM+13])
  26 #define err_bitmap      (hold_regs[MB_N_HOLD_REGS_EEPROM+14])
  27 #define max_distances   (hold_regs+MB_N_HOLD_REGS_EEPROM+21)
  28
  29 static void pull_trigger(uint8_t trig)
  30 {
  31         switch (trig) {
  32         case 0: PORTD |= _BV(PD7); _delay_us(10); PORTD &= ~_BV(PD7); break;
  33         case 1: PORTB |= _BV(PB4); _delay_us(10); PORTB &= ~_BV(PB4); break;
  34         case 2: PORTC |= _BV(PC4); _delay_us(10); PORTC &= ~_BV(PC4); break;
  35         }
  36 }
  37
  38 static uint16_t get_pin(uint8_t trig)
  39 {
  40         switch (trig) {
  41         case 0: return (PIND & 0x78) >> 3;
  42         case 1: return PINB & 0x0F;
  43         default: return PINC & 0x0F;
  44         }
  45 }
  46
  47 static void do_measurement(unsigned char trig)
  48 {
  49         uint16_t starttimes[N_TRIG_SENSORS], starttime;
  50         uint8_t to_start = (1 << N_TRIG_SENSORS) - 1;
  51         uint8_t to_measure = 0, i;
  52
  53         pull_trigger(trig);
  54
  55         starttime = get_clock();
  56
  57         while (to_start || to_measure) {
  58                 uint8_t bits = 0;
  59                 uint16_t now = get_clock();
  60
  61                 if (now-starttime >= ECHO_TIMEOUT)
  62                         break;
  63
  64                 bits = get_pin(trig);
  65
  66                 for (i = 0; i < N_TRIG_SENSORS; i++) {
  67                         uint8_t mask = 1 << i;
  68
  69                         if ((to_start & mask) && (bits & mask)) {
  70                                 // echo start
  71                                 starttimes[i] = now;
  72                                 to_start &= ~mask;
  73                                 to_measure |= mask;
  74                         } else if ((to_measure & mask) && !(bits & mask)) {
  75                                 uint16_t old_d, new_d;
  76                                 uint8_t idx = trig*N_TRIG_SENSORS+i;
  77                                 // echo end
  78                                 to_measure &= ~mask;
  79                                 new_d = now - starttimes[i];
  80                                 old_d = distances[idx];
  81
  82                                 if (new_d > max_distances[idx])
  83                                         max_distances[idx] = new_d;
  84
  85                                 if (old_d == 0
  86                                         || old_d == -1) {
  87                                         distances[idx] = new_d;
  88                                 } else {
  89                                         distances[idx] = (
  90                                                 (old_d << MEASUREMENT_SHIFT)
  91                                                 + new_d
  92                                                 - old_d
  93                                                 ) >> MEASUREMENT_SHIFT;
  94                                 }
  95                         }
  96                 }
  97         }
  98
  99         for (i = 0; i < N_TRIG_SENSORS; i++)
 100                 if (to_start & (1 << i))
 101                         distances[trig*N_TRIG_SENSORS + i] = -1;
 102                 else if (to_measure & (1 << i))
 103                         distances[trig*N_TRIG_SENSORS + i] = 0;
 104 }
 105
 106 static void led_set(uint8_t led, uint8_t state)
 107 {
 108         if (led == 0) {
 109                 switch (state) {
 110                 case 0:
 111                         led_bitmap &= ~1;
 112                         led_bitmap &= ~2;
 113                         break;
 114                 case 1:
 115                         led_bitmap |= 1;
 116                         led_bitmap &= ~2;
 117                         break;
 118                 default: // error
 119                         led_bitmap |= 2;
 120                         break;
 121                 }
 122         } else {
 123                 switch (state) {
 124                 case 0:
 125                         led_bitmap &= ~4;
 126                         led_bitmap &= ~8;
 127                         break;
 128                 case 1:
 129                         led_bitmap |= 4;
 130                         led_bitmap &= ~8;
 131                         break;
 132                 default:
 133                         led_bitmap |= 8;
 134                         break;
 135                 }
 136         }
 137 }
 138
 139 static void leds_update()
 140 {
 141         if (led_bitmap & 1) {
 142                 PORTC |= _BV(PC5);
 143         } else {
 144                 PORTC &= ~_BV(PC5);
 145         }
 146
 147         if (led_bitmap & 2) {
 148                 DDRC &= ~_BV(PC5);
 149         } else {
 150                 DDRC |= _BV(PC5);
 151         }
 152
 153         if (led_bitmap & 4) {
 154                 PORTB |= _BV(PB5);
 155         } else {
 156                 PORTB &= ~_BV(PB5);
 157         }
 158
 159         if (led_bitmap & 8) {
 160                 DDRB |= _BV(PB5);
 161         } else {
 162                 DDRB &= ~_BV(PB5);
 163         }
 164 }
 165
 166 static void eval_bitmaps()
 167 {
 168         uint16_t free_b = 0, err_b = 0, mask;
 169         uint8_t i;
 170
 171         for (i = 0; i < N_SENSORS; i++) {
 172                 mask = 1 << i;
 173
 174                 if (thresholds[i]) {
 175                         if (distances[i] == -1 || distances[i] == 0) {
 176                                 err_b |= mask;
 177                         } else if (distances[i] > thresholds[i]) {
 178                                 free_b |= mask;
 179                         }
 180                 }
 181         }
 182
 183         free_bitmap = free_b;
 184         err_bitmap  = err_b;
 185
 186         if (led1_sensors) {
 187                 if (led1_sensors & err_bitmap) {
 188                         led_set(0, 2);
 189                 } else if (led1_sensors & free_bitmap) {
 190                         led_set(0, 1);
 191                 } else {
 192                         led_set(0, 0);
 193                 }
 194         }
 195
 196         if (led2_sensors) {
 197                 if (led2_sensors & err_bitmap) {
 198                         led_set(1, 2);
 199                 } else if (led2_sensors & free_bitmap) {
 200                         led_set(1, 1);
 201                 } else {
 202                         led_set(1, 0);
 203                 }
 204         }
 205 }
 206
 207 uint8_t hold_reg_is_valid(uint16_t reg, uint16_t val)
 208 {
 209         if (reg == MB_HOLD_REGS_BASE)
 210                 return val > 0 && val <= 247;
 211
 212         return 1;
 213 }
 214
 215 int main()
 216 {
 217         modbus_init(0);
 218
 219         // output pins
 220         DDRD |= _BV(PD7); // Trig D
 221         DDRB |= _BV(PB4) | _BV(PB5); // Trig B, LED 2
 222         DDRC |= _BV(PC4) | _BV(PC5); // Trig C, LED 1
 223
 224         // set up the timer
 225         TCCR1A = 0;
 226         TCCR1B = _BV(CS12)|_BV(CS10); // CLK/1024
 227
 228         // enable interrupts
 229         sei();
 230
 231         while(1) {
 232                 uint8_t trig;
 233                 for (trig = 0; trig < N_TRIGGERS; trig++) {
 234                         uint16_t now;
 235                         do_measurement(trig);
 236                         now = get_clock();
 237                         while (get_clock()-now < MEASUREMENT_WAIT)
 238                                 modbus_poll();
 239                 }
 240
 241                 eval_bitmaps();
 242                 leds_update(); // might be written from modbus
 243 //              led_set(0,
 244 //                      distances[4] > 100 || distances[11] > 100);
 245         }
 246 }
 247