hacked in my disambiguator

7 files changed, 150 insertions, 132 deletions

diff --git a/Makefile b/Makefile
index dbd4869..90e0ec1 100644
--- a/Makefile
+++ b/Makefile
@@ -12,7 +12,7 @@ data_recorder : data_recorder.c lib/libsurvive.so redist/os_generic.o redist/Dra
 lib:
 	mkdir lib
 
-lib/libsurvive.so : src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o redist/jsmn.o $(DEBUGSTUFF)
+lib/libsurvive.so : src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o src/disambiguator.c redist/jsmn.o $(DEBUGSTUFF)
 	gcc -o $@ $^ $(LDFLAGS) -shared
 
 clean :
diff --git a/include/disambiguator.h b/include/disambiguator.h
new file mode 100644
index 0000000..58bce48
--- /dev/null
+++ b/include/disambiguator.h
@@ -0,0 +1,35 @@
+#ifndef DISAMBIGUATOR_H
+#define DISAMBIGUATOR_H
+
+#define DIS_NUM_VALUES 8
+
+typedef enum {
+	D_STATE_INVALID = 0,
+	D_STATE_LOCKED = 1,
+	D_STATE_UNLOCKED = -1,
+} dis_state;
+
+typedef enum {
+	P_UNKNOWN = 0,
+	P_SYNC = 1,
+	P_SWEEP = 2,
+} pulse_type;
+
+typedef struct disambiguator_ {
+	long times[DIS_NUM_VALUES];
+	int scores[DIS_NUM_VALUES];
+	dis_state state;
+	long last;
+	int max_confidence;
+	char code;
+} disambiguator;
+
+typedef struct classified_pulse_ {
+	pulse_type t;
+	int length;
+} classified_pulse;
+
+void disambiguator_init(disambiguator * d);
+pulse_type disambiguator_step(disambiguator * d, long time, int length);
+
+#endif /* DISAMBIGUATOR_H */
\ No newline at end of file
diff --git a/include/survive.h b/include/survive.h
index f64091f..e29c82b 100644
--- a/include/survive.h
+++ b/include/survive.h
@@ -1,6 +1,7 @@
 #ifndef _SURVIVE_H
 #define _SURVIVE_H
 
+#include "disambiguator.h"
 #include <stdint.h>
 
 #define SV_FLOAT  		double
@@ -31,10 +32,14 @@ struct SurviveObject
 
 	//Flood info, for calculating which laser is currently sweeping.
 	int8_t oldcode;
+#if 0
 	int16_t total_photos;
 	int32_t last_photo_time;
 	int32_t total_photo_time;
 	int32_t total_pulsecode_time;
+#else
+	disambiguator d;
+#endif
 };
 
 typedef void (*text_feedback_fnptr)( struct SurviveContext * ctx, const char * fault );
diff --git a/src/disambiguator.c b/src/disambiguator.c
new file mode 100644
index 0000000..9b55fa4
--- /dev/null
+++ b/src/disambiguator.c
@@ -0,0 +1,83 @@
+#include "../include/disambiguator.h"
+#include <stdlib.h>
+#include <string.h>
+
+void disambiguator_init(disambiguator * d) {
+	memset(&(d->times), 0x0, sizeof(d->times));
+	memset(&(d->scores), 0x0, sizeof(d->scores));
+	d->state = D_STATE_UNLOCKED;
+	d->last = 0;
+	d->max_confidence = 0;
+}
+
+inline void disambiguator_discard(disambiguator * d, long age);
+
+void disambiguator_discard(disambiguator * d, long age) {
+	int confidence = 0;
+	for (unsigned int i = 0; i < DIS_NUM_VALUES; ++i) {
+		if (d->times[i] != 0 && d->times[i] < age) {
+			d->times[i] = 0;
+			d->scores[i] = 0;
+		} else {
+			if (d->scores[i] > confidence) {
+				confidence = d->scores[i];
+			}
+		}
+	}
+	d->max_confidence = confidence;
+}
+
+inline int disambiguator_find_nearest(disambiguator * d, long time, int max_diff);
+
+int disambiguator_find_nearest(disambiguator * d, long time, int max_diff) {
+	int diff = max_diff; // max allowed diff for a match
+	int idx = -1;
+	for (unsigned int i = 0; i < DIS_NUM_VALUES; ++i) {
+		if (d->times[i] == 0) continue;
+
+		int a = abs(d->times[i] - time);
+		if (a < diff) {
+			idx = i;
+			diff = a;
+		}
+	}
+
+	return idx;
+}
+
+pulse_type disambiguator_step(disambiguator * d, long time, int length) {
+	if (length < 2750) {
+		return d->state == D_STATE_LOCKED ? P_SWEEP : P_UNKNOWN;
+	}
+
+	disambiguator_discard(d, time - 10000000);
+	int idx = disambiguator_find_nearest(d, time - 400000, 100);
+
+	if (time - d->last > 401000) {
+		d->state = D_STATE_UNLOCKED;
+	}
+
+	if (idx == -1) {
+		for (int i = 0; i < DIS_NUM_VALUES; ++i) {
+			if (d->times[i] == 0) {
+				d->times[i] = time;
+				break;
+			}
+		}
+
+		return d->state == D_STATE_LOCKED ? P_SWEEP : P_UNKNOWN;
+	} else {
+		d->scores[idx]++;
+		if (d->scores[idx] >= 30) {
+			d->state = D_STATE_LOCKED;
+		}
+
+		d->times[idx] = time;
+		d->last = time;
+		return d->state == D_STATE_LOCKED ? (
+			d->scores[idx] >= d->max_confidence ? P_SYNC : P_SWEEP
+		) : P_UNKNOWN;
+	}
+
+	return d->state == D_STATE_LOCKED ? P_SWEEP : P_UNKNOWN;
+}
diff --git a/src/survive_data.c b/src/survive_data.c
index 660c3e1..fc86d9f 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -11,6 +11,7 @@
 //All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
 
 #include "survive_internal.h"
+#include "disambiguator.h"
 #include <stdint.h>
 #include <string.h>
 
@@ -41,6 +42,21 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
 	if( le->type != 0xfe || le->length < 50 ) return;
 	//le->timestamp += (le->length/2);
 
+	int32_t offset = le->timestamp - so->d.last;
+	switch(disambiguator_step(&(so->d), le->timestamp, le->length)) {
+		default:
+		case P_UNKNOWN:
+			// not currently locked
+		case P_SYNC:
+			ct->lightproc( so, le->sensor_id, -1, 0, le->timestamp, offset );
+			so->d.code = ((le->length+125)/250) - 12;
+			break;
+		case P_SWEEP:
+			if (so->d.gcode & 1) return;
+			ct->lightproc( so, le->sensor_id, so->d.code >> 1, offset, le->timestamp, le->length );
+			break;
+	}
+#if 0
 	if( le->length > 2100 ) //Pulse longer indicates a sync pulse.
 	{
 		int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_photo_time;
@@ -85,6 +101,7 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
 	{
 		//Runt pulse.
 	}
+#endif
 }
 
 
diff --git a/tools/disambiguate/Makefile b/tools/disambiguate/Makefile
index e719469..8e8661c 100644
--- a/tools/disambiguate/Makefile
+++ b/tools/disambiguate/Makefile
@@ -1,4 +1,4 @@
-disambiguate: disambiguate.c
+disambiguate: disambiguate.c ../../src/disambiguator.c
 	gcc -Wall -Wextra -Werror -g -o $@ $^ -lm
 
 test: disambiguate
diff --git a/tools/disambiguate/disambiguate.c b/tools/disambiguate/disambiguate.c
index 9a52c16..dcaf9d8 100644
--- a/tools/disambiguate/disambiguate.c
+++ b/tools/disambiguate/disambiguate.c
@@ -2,134 +2,7 @@
 #include <stdlib.h>
 #include <string.h>
 #include <math.h>
-
-#define DIS_NUM_VALUES 64
-
-typedef enum {
-	D_SYNC_J0 = 0,
-	D_SYNC_K0 = 1,
-	D_SYNC_J1 = 2,
-	D_SYNC_K1 = 3,
-	D_SYNC_J2 = 4,
-	D_SYNC_K2 = 5,
-	D_SYNC_J3 = 6,
-	D_SYNC_K3 = 7,
-} sync_pulse;
-
-typedef enum {
-	D_STATE_INVALID = 0,
-	D_STATE_LOCKED = 1,
-	D_STATE_UNLOCKED = -1,
-} dis_state;
-
-typedef enum {
-	P_UNKNOWN = 0,
-	P_SYNC = 1,
-	P_SWEEP = 2,
-} pulse_type;
-
-typedef struct disambiguator_ {
-	long times[DIS_NUM_VALUES];
-	int scores[DIS_NUM_VALUES];
-	dis_state state;
-	long last;
-	int max_confidence;
-} disambiguator;
-
-typedef struct classified_pulse_ {
-	pulse_type t;
-	int length;
-} classified_pulse;
-
-void disambiguator_init(disambiguator * d) {
-	memset(&(d->times), 0x0, sizeof(d->times));
-	memset(&(d->scores), 0x0, sizeof(d->times));
-	d->state = D_STATE_UNLOCKED;
-	d->last = 0;
-	d->max_confidence = 0;
-}
-
-void disambiguator_discard(disambiguator * d, long age) {
-	int confidence = 0;
-	for (unsigned int i = 0; i < DIS_NUM_VALUES; ++i) {
-		if (d->times[i] != 0 && d->times[i] < age) {
-			d->times[i] = 0;
-			d->scores[i] = 0;
-		} else {
-			if (d->scores[i] > confidence) {
-				confidence = d->scores[i];
-			}
-		}
-	}
-	d->max_confidence = confidence;
-}
-
-int disambiguator_find_nearest(disambiguator * d, long time, int max_diff) {
-	int diff = max_diff; // max allowed diff for a match
-	int idx = -1;
-	for (unsigned int i = 0; i < DIS_NUM_VALUES; ++i) {
-		if (d->times[i] == 0) continue;
-
-		int a = abs(d->times[i] - time);
-		if (a < diff) {
-			idx = i;
-			diff = a;
-		}
-	}
-
-	return idx;
-}
-
-pulse_type disambiguator_step(disambiguator * d, long time, int length) {
-	if (length < 2750) {
-		return d->state == D_STATE_LOCKED ? P_SWEEP : P_UNKNOWN;
-	}
-
-	disambiguator_discard(d, time - 10000000);
-	int idx = disambiguator_find_nearest(d, time - 400000, 100);
-
-	if (time - d->last > 401000) {
-		d->state = D_STATE_UNLOCKED;
-	}
-
-	if (idx == -1) {
-		for (int i = 0; i < DIS_NUM_VALUES; ++i) {
-			if (d->times[i] == 0) {
-				d->times[i] = time;
-				break;
-			}
-		}
-
-		return d->state == D_STATE_LOCKED ? P_SWEEP : P_UNKNOWN;
-	} else {
-		// double l = length;
-		// char cc = round(l / 500) - 6;
-		// printf("MATCH: %li %d %d (0b%d%d%d)\n", time - d->times[idx], d->scores[idx], length, (cc >> 2) & 0x1, (cc >> 1) & 0x1, cc & 0x1);
-		d->scores[idx]++;
-		if (d->scores[idx] >= 30) {
-			d->state = D_STATE_LOCKED;
-			// printf("MATCH: %li %d\n", time - d->times[idx], scores[idx]);
-		}
-/*
-		for (unsigned int i = 0; i < DIS_NUM_VALUES; ++i) {
-			if (d->scores[i] > 0 && d->times[i] != 0)
-				printf("TS %2d %li %d\n", i, d->times[i], d->scores[i]);
-		}
-*/
-		d->times[idx] = time;
-		d->last = time;
-		return d->state == D_STATE_LOCKED ? (
-			d->scores[idx] >= d->max_confidence ? P_SYNC : P_SWEEP
-		) : P_UNKNOWN;
-	}
-
-	return d->state == D_STATE_LOCKED ? P_SWEEP : P_UNKNOWN;
-}
-
-sync_pulse length_to_pulse(int length) {
-	double l = length - 3000;
-	return round(l/500);
-}
+#include "../../include/disambiguator.h"
 
 int main() {
 
@@ -139,7 +12,7 @@ int main() {
 		return -1;
 	}
 
-	long last = 0;
+	long last = 0, lastl = 0;
 
 	disambiguator d;
 	disambiguator_init(&d);
@@ -153,6 +26,10 @@ int main() {
 		if (fscanf(f, "%s %d %d %d %li", controller, &sensor, &unknown, &length, &time) != 5) {
 			break;
 		}
+		if (lastl > time) {
+			printf("BACKWARDS: %li %li\n", lastl, time);
+		}
+		lastl = time;
 
 		switch (disambiguator_step(&d, time, length)) {
 			default:
@@ -163,7 +40,8 @@ int main() {
 				{
 					double l = length;
 					char cc = round(l / 500) - 6;
-					printf("SYNC  %s %2d %10li    %c%d %10li\n", controller, sensor, time, cc & 0x1 ? 'j' : 'k', (cc >> 1) & 0x3, time-last);
+					int ll = (length+125)/250;
+					printf("SYNC  %s %2d %10li %5d %c%d %10li %d %d\n", controller, sensor, time, length, (cc & 0x1) ? 'k' : 'j', (cc >> 1) & 0x3, time-last, ll & 1, (ll >> 1) - 6);
 					last = time;
 				}
 				continue;