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       0               // 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 #define long_as_free    (hold_regs[15])
  23 #define long_thr        (hold_regs[16])
  24
  25 #define led_bitmap      (hold_regs[MB_N_HOLD_REGS_EEPROM])
  26 #define distances       (hold_regs+MB_N_HOLD_REGS_EEPROM+1)
  27 #define free_bitmap     (hold_regs[MB_N_HOLD_REGS_EEPROM+13])
  28 #define err_bitmap      (hold_regs[MB_N_HOLD_REGS_EEPROM+14])
  29 #define long_bitmap     (hold_regs[MB_N_HOLD_REGS_EEPROM+15])
  30 #define max_distances   (hold_regs+MB_N_HOLD_REGS_EEPROM+21)
  31 #define err_counts      (hold_regs+MB_N_HOLD_REGS_EEPROM+41)
  32
  33 static void pull_trigger(uint8_t trig)
  34 {
  35         switch (trig) {
  36         case 0: PORTD |= _BV(PD7); _delay_us(10); PORTD &= ~_BV(PD7); break;
  37         case 1: PORTB |= _BV(PB4); _delay_us(10); PORTB &= ~_BV(PB4); break;
  38         case 2: PORTC |= _BV(PC4); _delay_us(10); PORTC &= ~_BV(PC4); break;
  39         }
  40 }
  41
  42 static uint16_t get_pin(uint8_t trig)
  43 {
  44         switch (trig) {
  45         case 0: return (PIND & 0x78) >> 3;
  46         case 1: return PINB & 0x0F;
  47         default: return PINC & 0x0F;
  48         }
  49 }
  50
  51 static void do_measurement(unsigned char trig)
  52 {
  53         uint16_t starttimes[N_TRIG_SENSORS], starttime;
  54         uint8_t to_start = (1 << N_TRIG_SENSORS) - 1;
  55         uint8_t to_measure = 0, i;
  56         uint16_t now;
  57
  58         pull_trigger(trig);
  59
  60         starttime = get_clock();
  61
  62         while (to_start || to_measure) {
  63                 uint8_t bits = 0;
  64                 now = get_clock();
  65
  66                 if (now-starttime >= ECHO_TIMEOUT)
  67                         break;
  68
  69                 bits = get_pin(trig);
  70
  71                 for (i = 0; i < N_TRIG_SENSORS; i++) {
  72                         uint8_t mask = 1 << i;
  73
  74                         if ((to_start & mask) && (bits & mask)) {
  75                                 // echo start
  76                                 starttimes[i] = now;
  77                                 to_start &= ~mask;
  78                                 to_measure |= mask;
  79                         } else if ((to_measure & mask) && !(bits & mask)) {
  80 #if MEASUREMENT_SHIFT > 0
  81                                 uint16_t old_d;
  82 #endif
  83                                 uint16_t new_d;
  84                                 uint8_t idx = trig*N_TRIG_SENSORS+i;
  85                                 // echo end
  86                                 to_measure &= ~mask;
  87                                 new_d = now - starttimes[i];
  88                                 if (new_d > max_distances[idx])
  89                                         max_distances[idx] = new_d;
  90
  91 #if MEASUREMENT_SHIFT > 0
  92                                 old_d = distances[idx];
  93
  94                                 if (old_d == 0
  95                                         || old_d == -1) {
  96                                         distances[idx] = new_d;
  97                                 } else {
  98                                         distances[idx] = (
  99                                                 (old_d << MEASUREMENT_SHIFT)
 100                                                 + new_d
 101                                                 - old_d
 102                                                 ) >> MEASUREMENT_SHIFT;
 103                                 }
 104 #else
 105                                 distances[idx] = new_d;
 106 #endif
 107                         }
 108                 }
 109         }
 110
 111         for (i = 0; i < N_TRIG_SENSORS; i++) {
 112                 uint8_t off = trig*N_TRIG_SENSORS + i;
 113
 114                 if (to_start & (1 << i)) { // echo not received
 115                         uint16_t err_count = err_counts[off] & 0xFF;
 116                         if (distances[off] != -1 && err_count < 255) {
 117                                 err_count++;
 118                                 err_counts[off] = (err_counts[off] & 0xFF00)
 119                                         | err_count;
 120                         }
 121                         distances[off] = -1;
 122                 } else if (to_measure & (1 << i)) { // echo pulse too long
 123                         uint16_t err_count = err_counts[off] >> 8;
 124
 125                         if (err_count < 255) {
 126                                 err_count++;
 127                                 err_counts[off] = (err_counts[off] & 0x00FF)
 128                                         | (err_count << 8);
 129                         }
 130                         /*
 131                          * If the echo pulse is too long, do not treat it
 132                          * as error, just count it as maximum length
 133                          * and notify the state in the bitmap.
 134                          */
 135                         distances[off] = now - starttimes[i];
 136                 }
 137         }
 138 }
 139
 140 static void led_set(uint8_t led, uint8_t state)
 141 {
 142         if (led == 0) {
 143                 switch (state) {
 144                 case 0:
 145                         led_bitmap &= ~1;
 146                         led_bitmap &= ~2;
 147                         break;
 148                 case 1:
 149                         led_bitmap |= 1;
 150                         led_bitmap &= ~2;
 151                         break;
 152                 default: // error
 153                         led_bitmap |= 2;
 154                         break;
 155                 }
 156         } else {
 157                 switch (state) {
 158                 case 0:
 159                         led_bitmap &= ~4;
 160                         led_bitmap &= ~8;
 161                         break;
 162                 case 1:
 163                         led_bitmap |= 4;
 164                         led_bitmap &= ~8;
 165                         break;
 166                 default:
 167                         led_bitmap |= 8;
 168                         break;
 169                 }
 170         }
 171 }
 172
 173 static void leds_update()
 174 {
 175         if (led_bitmap & 1) {
 176                 PORTC |= _BV(PC5);
 177         } else {
 178                 PORTC &= ~_BV(PC5);
 179         }
 180
 181         if (led_bitmap & 2) {
 182                 DDRC &= ~_BV(PC5);
 183         } else {
 184                 DDRC |= _BV(PC5);
 185         }
 186
 187         if (led_bitmap & 4) {
 188                 PORTB |= _BV(PB5);
 189         } else {
 190                 PORTB &= ~_BV(PB5);
 191         }
 192
 193         if (led_bitmap & 8) {
 194                 DDRB &= ~_BV(PB5);
 195         } else {
 196                 DDRB |= _BV(PB5);
 197         }
 198 }
 199
 200 static void eval_bitmaps()
 201 {
 202         uint16_t free_b = 0, err_b = 0, long_b = 0, mask;
 203         uint8_t i;
 204
 205         for (i = 0; i < N_SENSORS; i++) {
 206                 mask = 1 << i;
 207
 208                 if (thresholds[i]) {
 209                         if (distances[i] == -1) {
 210                                 err_b |= mask;
 211                         } else if (distances[i] > thresholds[i]) {
 212                                 if (long_thr && distances[i] > long_thr) {
 213                                         long_b |= mask;
 214                                         if (long_as_free & mask) {
 215                                                 free_b |= mask;
 216                                         }
 217                                 } else {
 218                                         free_b |= mask;
 219                                 }
 220                         }
 221                 }
 222         }
 223
 224         free_bitmap = free_b;
 225         err_bitmap  = err_b;
 226         long_bitmap = long_b;
 227
 228         if (led1_sensors) {
 229                 if (led1_sensors & err_bitmap) {
 230                         led_set(0, 2);
 231                 } else if (led1_sensors & free_bitmap) {
 232                         led_set(0, 1);
 233                 } else {
 234                         led_set(0, 0);
 235                 }
 236         }
 237
 238         if (led2_sensors) {
 239                 if (led2_sensors & err_bitmap) {
 240                         led_set(1, 2);
 241                 } else if (led2_sensors & free_bitmap) {
 242                         led_set(1, 1);
 243                 } else {
 244                         led_set(1, 0);
 245                 }
 246         }
 247 }
 248
 249 uint8_t hold_reg_is_valid(uint16_t reg, uint16_t val)
 250 {
 251         if (reg == MB_HOLD_REGS_BASE)
 252                 return val > 0 && val <= 247;
 253
 254         return 1;
 255 }
 256
 257 int main()
 258 {
 259         modbus_init(0);
 260
 261         // output pins
 262         DDRD |= _BV(PD7); // Trig D
 263         DDRB |= _BV(PB4) | _BV(PB5); // Trig B, LED 2
 264         DDRC |= _BV(PC4) | _BV(PC5); // Trig C, LED 1
 265
 266         // set up the timer
 267         TCCR1A = 0;
 268         TCCR1B = _BV(CS12)|_BV(CS10); // CLK/1024
 269
 270         // enable interrupts
 271         sei();
 272
 273         while(1) {
 274                 uint8_t trig;
 275                 for (trig = 0; trig < N_TRIGGERS; trig++) {
 276                         uint16_t now;
 277                         do_measurement(trig);
 278                         now = get_clock();
 279                         while (get_clock()-now < MEASUREMENT_WAIT)
 280                                 modbus_poll();
 281                 }
 282
 283                 eval_bitmaps();
 284                 leds_update(); // might be written from modbus
 285 //              led_set(0,
 286 //                      distances[4] > 100 || distances[11] > 100);
 287         }
 288 }
 289