hacked in my disambiguator

2 files changed, 100 insertions, 0 deletions

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
 }