X-Git-Url: https://www.fi.muni.cz/~kas/git//home/kas/public_html/git/?a=blobdiff_plain;f=firmware%2Ffirmware.c;h=7fbcdf18601b65f661edfb46b9cced58d937a72a;hb=HEAD;hp=c9c4317db1de3d1571498dde1ee45f975488aca4;hpb=dc770d3e8804b9cb81fd1d0c7fc7336a14dd7c1f;p=openparking.git

diff --git a/firmware/firmware.c b/firmware/firmware.c
index c9c4317..7fbcdf1 100644
--- a/firmware/firmware.c
+++ b/firmware/firmware.c
@@ -5,9 +5,9 @@
 #include "clock.h"
 #include "modbus.h"
 
-#define ECHO_TIMEOUT		(CLOCK_HZ/20)	// 50 ms
-#define MEASUREMENT_WAIT	(4*ECHO_TIMEOUT)
-#define MEASUREMENT_SHIFT	2		// running avg (1 << M_SHIFT)
+#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
@@ -19,11 +19,16 @@
 #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)
 {
@@ -48,6 +53,7 @@ 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);
 
@@ -55,7 +61,7 @@ static void do_measurement(unsigned char trig)
 
 	while (to_start || to_measure) {
 		uint8_t bits = 0;
-		uint16_t now = get_clock();
+		now = get_clock();
 
 		if (now-starttime >= ECHO_TIMEOUT)
 			break;
@@ -71,11 +77,18 @@ static void do_measurement(unsigned char trig)
 				to_start &= ~mask;
 				to_measure |= mask;
 			} else if ((to_measure & mask) && !(bits & mask)) {
-				uint16_t old_d, new_d;
+#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;
 				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
@@ -88,15 +101,40 @@ static void do_measurement(unsigned char trig)
 						- 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;
+
+		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
+			 * and notify the state in the bitmap.
+			 */
+			distances[off] = now - starttimes[i];
+		}
+	}
 }
 
 static void led_set(uint8_t led, uint8_t state)
@@ -153,31 +191,39 @@ static void leds_update()
 	}
 
 	if (led_bitmap & 8) {
-		DDRB |= _BV(PB5);
-	} else {
 		DDRB &= ~_BV(PB5);
+	} else {
+		DDRB |= _BV(PB5);
 	}
 }
 
 static void eval_bitmaps()
 {
-	uint16_t free_b = 0, err_b = 0, mask;
+	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 || distances[i] == 0) {
+			if (distances[i] == -1) {
 				err_b |= mask;
 			} else if (distances[i] > thresholds[i]) {
-				free_b |= mask;
+				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) {