#include <avr/interrupt.h>
#include <util/delay.h>
#include <stdio.h>
+#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 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 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)
{
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);
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.
+ */
+ 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, 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]) {
+ free_b |= mask;
+ }
+ }
+ }
+
+ free_bitmap = free_b;
+ err_bitmap = err_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;
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);
}
}
https://www.fi.muni.cz/~kas/git / openparking.git / blobdiff