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=3554dcc768e38e6deda7bdd7fe5fb8459e52a2c2;hb=b6aa72c62905573d4558d11973b5180e112f44d8;hp=ce4a91486cda276c43235dca59fd77b373af6bd3;hpb=e9281b1cc49e6f5f43e45038f8f90f6f75c8a4a0;p=openparking.git diff --git a/firmware/firmware.c b/firmware/firmware.c index ce4a914..3554dcc 100644 --- a/firmware/firmware.c +++ b/firmware/firmware.c @@ -2,9 +2,12 @@ #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 @@ -21,6 +24,8 @@ #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 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) { @@ -45,16 +50,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); @@ -68,54 +74,125 @@ 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; -} - -static void do_measurements() -{ - uint8_t trig; + for (i = 0; i < N_TRIG_SENSORS; i++) { + uint8_t off = trig*N_TRIG_SENSORS + i; - for (trig = 0; trig < N_TRIGGERS; trig++) { - do_measurement(trig); - _delay_ms(200); + 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; + + 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. + */ + 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); - led_bitmap |= 1; - } 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); - led_bitmap |= 2; - } else { - PORTB &= ~_BV(PB5); - led_bitmap &= ~2; + 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, mask; @@ -125,7 +202,7 @@ static void eval_bitmaps() mask = 1 << i; if (thresholds[i]) { - if (distances[i] == -1 || distances[i] == 0) { + if (distances[i] == -1) { err_b |= mask; } else if (distances[i] > thresholds[i]) { free_b |= mask; @@ -137,7 +214,9 @@ static void eval_bitmaps() err_bitmap = err_b; if (led1_sensors) { - if (led1_sensors & free_bitmap) { + if (led1_sensors & err_bitmap) { + led_set(0, 2); + } else if (led1_sensors & free_bitmap) { led_set(0, 1); } else { led_set(0, 0); @@ -145,7 +224,9 @@ static void eval_bitmaps() } if (led2_sensors) { - if (led2_sensors & free_bitmap) { + if (led2_sensors & err_bitmap) { + led_set(1, 2); + } else if (led2_sensors & free_bitmap) { led_set(1, 1); } else { led_set(1, 0); @@ -169,8 +250,6 @@ int main() 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; @@ -184,12 +263,13 @@ int main() for (trig = 0; trig < N_TRIGGERS; trig++) { uint16_t now; do_measurement(trig); - now = TCNT1; - while (TCNT1-now < 200) + 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); }