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