X-Git-Url: https://www.fi.muni.cz/~kas/git//home/kas/public_html/git/?a=blobdiff_plain;ds=sidebyside;f=firmware%2Ffirmware.c;h=7fbcdf18601b65f661edfb46b9cced58d937a72a;hb=74ebbcd6ab77d57804b3548e1efa2ff86228e4be;hp=9b925210d48cbceee072333ea204e34a77cbfaab;hpb=86f8e5bfd1c753034b9ca876dac7d18720ab8808;p=openparking.git diff --git a/firmware/firmware.c b/firmware/firmware.c old mode 100755 new mode 100644 index 9b92521..7fbcdf1 --- a/firmware/firmware.c +++ b/firmware/firmware.c @@ -2,15 +2,33 @@ #include #include #include +#include "clock.h" #include "modbus.h" -#define TIMEOUT 0x2FF +#define ECHO_TIMEOUT (CLOCK_HZ/10) // 100 ms +#define MEASUREMENT_WAIT (2*ECHO_TIMEOUT) +#define MEASUREMENT_SHIFT 0 // running avg (1 << M_SHIFT) #define N_TRIGGERS 3 #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 long_as_free (hold_regs[15]) +#define long_thr (hold_regs[16]) + +#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]) +#define long_bitmap (hold_regs[MB_N_HOLD_REGS_EEPROM+15]) +#define max_distances (hold_regs+MB_N_HOLD_REGS_EEPROM+21) +#define err_counts (hold_regs+MB_N_HOLD_REGS_EEPROM+41) static void pull_trigger(uint8_t trig) { @@ -35,16 +53,17 @@ static void do_measurement(unsigned char trig) uint16_t starttimes[N_TRIG_SENSORS], starttime; uint8_t to_start = (1 << N_TRIG_SENSORS) - 1; uint8_t to_measure = 0, i; + uint16_t now; pull_trigger(trig); - starttime = TCNT1; + starttime = get_clock(); while (to_start || to_measure) { uint8_t bits = 0; - uint16_t now = TCNT1; + now = get_clock(); - if (now-starttime >= TIMEOUT) + if (now-starttime >= ECHO_TIMEOUT) break; bits = get_pin(trig); @@ -58,60 +77,191 @@ static void do_measurement(unsigned char trig) to_start &= ~mask; to_measure |= mask; } else if ((to_measure & mask) && !(bits & mask)) { +#if MEASUREMENT_SHIFT > 0 + uint16_t old_d; +#endif + uint16_t new_d; + uint8_t idx = trig*N_TRIG_SENSORS+i; // echo end to_measure &= ~mask; - distances[trig*N_TRIG_SENSORS + i] - = now - starttimes[i]; + new_d = now - starttimes[i]; + if (new_d > max_distances[idx]) + max_distances[idx] = new_d; + +#if MEASUREMENT_SHIFT > 0 + old_d = distances[idx]; + + if (old_d == 0 + || old_d == -1) { + distances[idx] = new_d; + } else { + distances[idx] = ( + (old_d << MEASUREMENT_SHIFT) + + new_d + - old_d + ) >> MEASUREMENT_SHIFT; + } +#else + distances[idx] = new_d; +#endif } } } - for (i = 0; i < N_TRIG_SENSORS; i++) - if (to_start & (1 << i)) - distances[trig*N_TRIG_SENSORS + i] = -1; - else if (to_measure & (1 << i)) - distances[trig*N_TRIG_SENSORS + i] = 0; -} + for (i = 0; i < N_TRIG_SENSORS; i++) { + uint8_t off = trig*N_TRIG_SENSORS + i; -static void do_measurements() -{ - uint8_t trig; + if (to_start & (1 << i)) { // echo not received + uint16_t err_count = err_counts[off] & 0xFF; + if (distances[off] != -1 && err_count < 255) { + err_count++; + err_counts[off] = (err_counts[off] & 0xFF00) + | err_count; + } + distances[off] = -1; + } else if (to_measure & (1 << i)) { // echo pulse too long + uint16_t err_count = err_counts[off] >> 8; - for (trig = 0; trig < N_TRIGGERS; trig++) { - do_measurement(trig); - _delay_ms(200); + if (err_count < 255) { + err_count++; + err_counts[off] = (err_counts[off] & 0x00FF) + | (err_count << 8); + } + /* + * If the echo pulse is too long, do not treat it + * as error, just count it as maximum length + * and notify the state in the bitmap. + */ + distances[off] = now - starttimes[i]; + } } } static void led_set(uint8_t led, uint8_t state) { if (led == 0) { - if (state) { - PORTD |= _BV(PD4); - // PORTC |= _BV(PC5); - } else { - PORTD &= ~_BV(PD4); - // PORTC &= ~_BV(PC5); + switch (state) { + case 0: + led_bitmap &= ~1; + led_bitmap &= ~2; + break; + case 1: + led_bitmap |= 1; + led_bitmap &= ~2; + break; + default: // error + led_bitmap |= 2; + break; } } else { - if (state) { - PORTB |= _BV(PB5); + switch (state) { + case 0: + led_bitmap &= ~4; + led_bitmap &= ~8; + break; + case 1: + led_bitmap |= 4; + led_bitmap &= ~8; + break; + default: + led_bitmap |= 8; + break; + } + } +} + +static void leds_update() +{ + if (led_bitmap & 1) { + PORTC |= _BV(PC5); + } else { + PORTC &= ~_BV(PC5); + } + + if (led_bitmap & 2) { + DDRC &= ~_BV(PC5); + } else { + DDRC |= _BV(PC5); + } + + if (led_bitmap & 4) { + PORTB |= _BV(PB5); + } else { + PORTB &= ~_BV(PB5); + } + + if (led_bitmap & 8) { + DDRB &= ~_BV(PB5); + } else { + DDRB |= _BV(PB5); + } +} + +static void eval_bitmaps() +{ + uint16_t free_b = 0, err_b = 0, long_b = 0, mask; + uint8_t i; + + for (i = 0; i < N_SENSORS; i++) { + mask = 1 << i; + + if (thresholds[i]) { + if (distances[i] == -1) { + err_b |= mask; + } else if (distances[i] > thresholds[i]) { + if (long_thr && distances[i] > long_thr) { + long_b |= mask; + if (long_as_free & mask) { + free_b |= mask; + } + } else { + free_b |= mask; + } + } + } + } + + free_bitmap = free_b; + err_bitmap = err_b; + long_bitmap = long_b; + + if (led1_sensors) { + if (led1_sensors & err_bitmap) { + led_set(0, 2); + } else if (led1_sensors & free_bitmap) { + led_set(0, 1); + } else { + led_set(0, 0); + } + } + + if (led2_sensors) { + if (led2_sensors & err_bitmap) { + led_set(1, 2); + } else if (led2_sensors & free_bitmap) { + led_set(1, 1); } else { - PORTB &= ~_BV(PB5); + 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 DDRB |= _BV(PB4) | _BV(PB5); // Trig B, LED 2 DDRC |= _BV(PC4) | _BV(PC5); // Trig C, LED 1 - // temporary LED - DDRD |= _BV(PD4); // set up the timer TCCR1A = 0; @@ -121,10 +271,19 @@ 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 = get_clock(); + while (get_clock()-now < MEASUREMENT_WAIT) + modbus_poll(); + } + + eval_bitmaps(); + leds_update(); // might be written from modbus +// led_set(0, +// distances[4] > 100 || distances[11] > 100); } }