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