Update disambiguator. I think I got it right this time, finally. Also, seems to output OOTX data pretty good.

4 files changed, 102 insertions, 357 deletions

diff --git a/src/disambiguator.c b/src/disambiguator.c
deleted file mode 100644
index f1d310a..0000000
--- a/src/disambiguator.c
+++ /dev/null
@@ -1,197 +0,0 @@
-// (C) 2016 Julian Picht, MIT/x11 License.
-//
-// All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
-
-//
-// The theory behind this disambiguator is, that if we just track all pulses and if one could be a sync pulse, we look back in time,
-// if we saw as sync pulse X samples ago than it is probably a sync pulse.
-//
-// X can be 20000 or 400000, depending if it came from the master or the slave.
-//
-
-#include "disambiguator.h"
-#include <stdlib.h>
-#include <string.h>
-#include <stdio.h>
-#include <stdbool.h>
-
-typedef uint8_t pulse_data;
-
-/**
- * Translate pulse length to pulse SKIP, DATA, AXIS
- * @param length Length of the pulse in (1/48000000) seconds
- * @return pulse data
- */
-pulse_data get_pulse_data(uint32_t length) {
-	uint16_t temp = length - 2880;
-
-#if BETTER_SAFE_THAN_FAST
-	if (temp < 0 || length > 6525) {
-		return -1;
-	}
-#endif
-
-	if ((temp % 500) < 150) {
-		return temp / 500;
-	}
-
-	return -1;
-}
-
-const uint32_t pulse_types[] = {
-	0, 1, 0, 1,
-	2, 3, 2, 3,
-};
-
-#define PULSE_BIT_AXIS 0x1
-#define PULSE_BIT_DATA 0x2
-#define PULSE_BIT_SKIP 0x4
-
-#define PULSE_DATA(D) ((D >> 1)&0x1)
-#define PULSE_AXIS(D) (D&0x01)
-#define PULSE_SKIP(D) ((D >> 2)&0x1)
-
-void disambiguator_init( struct 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( struct disambiguator * d );
-
-/**
- * Drop all data that is outdated
- * @param d
- * @param age Maximum age of data we care to keep
- */
-void disambiguator_discard( struct disambiguator * d )
-{
-	long age;
-	if (d->state == D_STATE_LOCKED) {
-		age = d->last - 400000;
-	} else {
-		age = 1000000;
-	}
-	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;
-}
-
-/**
- * Find the index that has the best likelyhood too match up with the timestamp given
- * @param t1 Rising edge time, where we expect to find the last sync pulse, if this is a master pulse
- * @param t2 Rising edge time, where we expect to find the last sync pulse, if this is a slave pulse
- * @param max_diff Maximum difference we are prepared to accept
- * @return index inside d->times, if we found something, -1 otherwise
- */
-inline int disambiguator_find_nearest( struct disambiguator * d, uint32_t t1, uint32_t t2, int max_diff );
-
-int disambiguator_find_nearest( struct disambiguator * d, uint32_t t1, uint32_t t2, 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_1 = abs(d->times[i] - t1);
-		int a_2 = abs(d->times[i] - t2);
-
-//		printf("T            %d %d %d\n", time, i, a);
-		if (a_1 < diff) {
-			idx = i;
-			diff = a_1;
-		} else if (a_2 < diff) {
-			idx = i;
-			diff = a_2;
-		}
-	}
-
-//	if (idx != -1) {
-//		printf("R            %d %d %d\n", idx, d->scores[idx], diff);
-//	}
-
-	return idx;
-}
-
-pulse_type disambiguator_step_return_helper( struct disambiguator * d, bool sweep_possible ) {
-	if (d->state == D_STATE_LOCKED && sweep_possible) {
-		return P_SWEEP;
-	}
-	return P_UNKNOWN;
-}
-
-pulse_type disambiguator_step( struct disambiguator * d, uint32_t time, int length)
-{
-	uint32_t diff = time - d->last;
-	bool sweep_possible = (diff > 70000 && diff < 350000);
-
-	// all smaller pulses are most probably sweeps
-	// TODO: check we are inside the time window of actual sweeps
-	if (length < 2750) {
-		return disambiguator_step_return_helper(d, sweep_possible);
-	}
-
-	// we expected to see a sync pulse earlier ...
-	if (time - d->last > 401000) {
-		d->state = D_STATE_UNLOCKED;
-	}
-
-	// discard all data, that is so old, we don't care about it anymore
-	disambiguator_discard(d);
-
-	// find the best match for our timestamp and presumed offset
-	int idx = disambiguator_find_nearest(d, time - 400000, time - 20000, 1000);
-
-	// We did not find a matching pulse, so try find a place to record the current
-	// one's time of arrival.
-	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 && sweep_possible ? P_SWEEP : P_UNKNOWN;
-	} else {
-		d->scores[idx]++;
-
-		// we need to be reasonably sure, that we have the right pulses
-		if (d->scores[idx] >= DIS_NUM_PULSES_BEFORE_LOCK) {
-			d->state = D_STATE_LOCKED;
-		}
-
-		// if the offset is about 20000 ticks, then this is a slave pulse
-		if (diff < 21000) {
-			if (d->state == D_STATE_LOCKED) {
-				return P_SLAVE;
-			}
-
-			return P_UNKNOWN;
-		}
-
-		d->times[idx] = time;
-		d->last = time;
-
-		// TODO: why do we need to check the confidence level here?
-		if (d->state == D_STATE_LOCKED && d->scores[idx] >= d->max_confidence) {
-			return P_MASTER;
-		}
-
-		return P_UNKNOWN;
-	}
-
-	return disambiguator_step_return_helper(d, sweep_possible);
-}
diff --git a/src/disambiguator.h b/src/disambiguator.h
deleted file mode 100644
index 8258a18..0000000
--- a/src/disambiguator.h
+++ /dev/null
@@ -1,68 +0,0 @@
-// (C) 2016 Julian Picht, MIT/x11 License.
-//
-//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
-#ifndef DISAMBIGUATOR_H
-#define DISAMBIGUATOR_H
-
-// Determines the number of samples stored in the disambiguator struct.
-// Has to be higher than the maximum number of pulses expected between sync pulses.
-#define DIS_NUM_VALUES 48
-#define DIS_NUM_PULSES_BEFORE_LOCK 30
-#include <stdint.h>
-
-/**
- * internal disambiguator state
- */
-typedef enum {
-	D_STATE_INVALID = 0,
-	D_STATE_LOCKED = 1,
-	D_STATE_UNLOCKED = -1,
-} dis_state;
-
-/**
- * classification result
- */
-typedef enum {
-	P_UNKNOWN = 0,
-	P_MASTER = 1,
-	P_SWEEP = 2,
-	P_SLAVE = 3,
-} pulse_type;
-
-/**
- * internal state of the disambiguator
- */
-struct disambiguator {
-	// the timestamps of the recorded pulses
-	uint32_t times[DIS_NUM_VALUES];
-	// countes how many sync pulses we have seen, that match the time offset at the same offset
-	uint16_t scores[DIS_NUM_VALUES];
-	// current state
-	dis_state state;
-	// last sync pulse time
-	uint32_t last;
-	// the absolute maximum counter value
-	int max_confidence;
-	// the last code type seen
-	char code;
-};
-
-
-/**
- * Initialize a new disambiguator. calloc or memset with 0x00 will work just as well.
- *
- * @param d Pointer to the struct
- */
-void disambiguator_init( struct disambiguator * d);
-
-/**
- * Feed in one pulse to have if classified.
- *
- * @param d      Pointer to disambiguator state
- * @param time   Rising edge of the pulse
- * @param length Length of the pulse
- * @return       Classification result
- */
-pulse_type disambiguator_step( struct disambiguator * d, uint32_t time, int length);
-
-#endif /* DISAMBIGUATOR_H */
\ No newline at end of file
diff --git a/src/survive.c b/src/survive.c
index af4d804..ed18da2 100644
--- a/src/survive.c
+++ b/src/survive.c
@@ -8,7 +8,7 @@
 #include <jsmn.h>
 #include <string.h>
 #include <zlib.h>
-#include "disambiguator.h"
+
 
 static int jsoneq(const char *json, jsmntok_t *tok, const char *s) {
  if (tok->type == JSMN_STRING && (int) strlen(s) == tok->end - tok->start &&
@@ -147,21 +147,10 @@ struct SurviveContext * survive_init()
 
 	ctx->headset.ctx = ctx;
 	memcpy( ctx->headset.codename, "HMD", 4 );
-#ifndef USE_OLD_DISAMBIGUATOR
-	ctx->headset.d = calloc( 1, sizeof( struct disambiguator ) );
-#endif
-
 	ctx->watchman[0].ctx = ctx;
 	memcpy( ctx->watchman[0].codename, "WM0", 4 );
-#ifndef USE_OLD_DISAMBIGUATOR
-	ctx->watchman[0].d = calloc( 1, sizeof( struct disambiguator ) );
-#endif
-
 	ctx->watchman[1].ctx = ctx;
 	memcpy( ctx->watchman[1].codename, "WM1", 4 );
-#ifndef USE_OLD_DISAMBIGUATOR
-	ctx->watchman[1].d = calloc( 1, sizeof( struct disambiguator ) );
-#endif
 
 	//USB must happen last.
 	if( r = survive_usb_init( ctx ) )
diff --git a/src/survive_data.c b/src/survive_data.c
index 808c891..afda235 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -11,7 +11,6 @@
 //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>
 
@@ -32,125 +31,148 @@ struct LightcapElement
 //This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode.
 static void handle_lightcap( struct SurviveObject * so, struct LightcapElement * le )
 {
-	struct SurviveContext * ct = so->ctx;
-	int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time;
+	struct SurviveContext * ctx = so->ctx;
+	//int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time;
 
 //	printf( "%s %d %d %d %d %d\n", so->codename, le->sensor_id, le->type, le->length, le->timestamp, le->timestamp-so->tsl );
 
 	so->tsl = le->timestamp;
 	if( le->length < 20 ) return;
-#ifndef USE_OLD_DISAMBIGUATOR
-	int32_t offset = le->timestamp - so->d->last;
-	switch( disambiguator_step( so->d, le->timestamp, le->length ) ) {
-	default:
-	case P_SLAVE:
-		// this is only interesting for the OOTX data
-		break;
-	case P_UNKNOWN:
-		// not currently locked
-		break;
-	case P_MASTER:
-		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->code & 1) return;
-		ct->lightproc( so, le->sensor_id, so->d->code >> 1, offset, le->timestamp, le->length );
-		break;
+
+	//The sync pulse finder is taking Charles's old disambiguator code and mixing it with a more linear
+	//version of Julian Picht's disambiguator, available in 488c5e9.  Removed afterwards into this
+	//unified driver.
+
+
+	int ssn = so->sync_set_number;
+	if( ssn < 0 ) ssn = 0;
+	int last_sync_time  =  so->last_time  [ssn];
+	int last_sync_length = so->last_length[ssn];
+	int32_t delta = le->timestamp - last_sync_time;  //Handle time wrapping (be sure to be int32)
+
+	if( delta < -500000 || delta > 500000 )
+	{
+		//Reset pulse, etc.
+		so->sync_set_number = -1;
+		delta = 500000;
 	}
-#else
+
+
 	if( le->length > 2200 ) //Pulse longer indicates a sync pulse.
 	{
-		int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time;
-		//        if( so->codename[0] == 'W' )
+		int is_new_pulse = delta > 1500 + last_sync_length;
+
+		so->did_handle_ootx = 0;
 
-		if( deltat > 2000 )
+		if( is_new_pulse )
 		{
-			//YUGH!!  This is really ugly, should refactor.
+			int is_master_sync_pulse = delta > 40000;
 
-			if( so->is_on_slave )
+			if( is_master_sync_pulse )
 			{
-				//Last was a slave.  If new pulse, switch back to master mode.
-				if( le->timestamp - so->last_slave_time > 1500 + so->last_slave_length )
-				{
-					so->is_on_slave = 0;
-					so->last_master_time = le->timestamp;
-					so->last_master_length = le->length;
-					ct->lightproc( so, le->sensor_id, -1, 0, le->timestamp, deltat );
-				}
+				ssn = so->sync_set_number = 0;
+				so->last_time[ssn] = le->timestamp;
+				so->last_length[ssn] = le->length;
+			}
+			else if( so->sync_set_number == -1 )
+			{
+				//Do nothing.
 			}
 			else
 			{
-				//See if this is a unique pulse, or another one in the same set we need to look at.
-				if( le->timestamp - so->last_master_time > 1500 + so->last_master_length )
+				ssn = ++so->sync_set_number;
+				if( so->sync_set_number > 1 )
 				{
-					// check if it is a slave pulse
-					if (le->timestamp - so->last_master_time < 70000) {
-						so->last_slave_time = le->timestamp;
-						so->last_slave_length = le->length;
-						so->is_on_slave = 1;
-						return;
-					}
-
-					so->is_on_slave = 0;
-					so->last_master_time = le->timestamp;
-					so->last_master_length = le->length;
-					ct->lightproc( so, le->sensor_id, -1, 0, le->timestamp, deltat );
-					deltat = 0;
+					SV_INFO( "Warning.  Received an extra, unassociated sync pulse." );
+					ssn = so->sync_set_number = -1;
+				}
+				else
+				{
+					so->last_time[ssn] = le->timestamp;
+					so->last_length[ssn] = le->length;
 				}
-			}
-		}
-
-		//Find longest pulse-length from device in our window and use that one.
-		if( so->is_on_slave )
-		{
-//			printf("%10u %10u %6d %6d\n", so->last_master_time, le->timestamp, (le->length - 2750)/500, (int32_t)le->timestamp - (int32_t)so->last_master_time);
-
-			if( le->length > so->last_slave_length )
-			{
-				so->last_slave_time = le->timestamp;
-				so->last_slave_length = le->length;
 			}
 		}
 		else
 		{
-			if( le->length > so->last_master_length )
+			//Find the longest pulse.
+			if( le->length > last_sync_length )
 			{
-				so->last_master_time = le->timestamp;
-				so->last_master_length = le->length;
+				if( so->last_time[ssn] > le->timestamp )
+				{
+					so->last_time[ssn] = le->timestamp;
+					so->last_length[ssn] = le->length;
+				}
 			}
 		}
 	}
-
 	//See if this is a valid actual pulse.
-	else if( le->length < 1800 && le->length > 40 && ( le->timestamp - so->last_master_time < 380000 ) )
+	else if( le->length < 1800 && le->length > 40 && delta > 30000 && ssn >= 0 )
 	{
-		int32_t dl = so->last_master_time;
-		int32_t tpco = so->last_master_length;
+		int32_t dl = so->last_time[0];
+		int32_t tpco = so->last_length[0];
 
 		//Adding length 
 		//Long pulse-code from IR flood.
 		//Make sure it fits nicely into a divisible-by-500 time.
-		int32_t acode = (tpco+125+50)/250;  //+10, seems ike that's
-		if( acode & 1 ) return;
+		int32_t acode_array[2] =
+			{
+				(so->last_length[0]+125+50)/250,
+				(so->last_length[1]+125+50)/250,
+			};
 
-		acode >>= 1;
-		acode -= 6;
+		//XXX: TODO: Capture error count here.
+		if( acode_array[0] & 1 ) return;
+		if( acode_array[1] & 1 ) return;
 
-		if (acode > 3) {
-			return;
-			dl = so->last_slave_time;
-			tpco = so->last_slave_length;
+		acode_array[0] = (acode_array[0]>>1) - 6;
+		acode_array[1] = (acode_array[1]>>1) - 6;
+
+		int acode = acode_array[0];
+
+		if( !so->did_handle_ootx )
+		{
+			int32_t delta1 = so->last_time[0] - so->recent_sync_time;
+			int32_t delta2 = so->last_time[1] - so->last_time[0];
+
+			ctx->lightproc( so, -1, acode_array[0], delta1, so->last_time[0], so->last_length[0] );
+			ctx->lightproc( so, -2, acode_array[1], delta2, so->last_time[1], so->last_length[1] );
+			so->recent_sync_time = so->last_time[1];
+
+			//Throw out everything if our sync pulses look like they're bad.
+			if( delta1 < 375000 || delta1 > 385000 )
+			{
+				//XXX: TODO: Count faults.
+				so->sync_set_number = -1;
+				return;
+			}
+
+			if( delta2 < 15000 || delta2 > 25000 )
+			{
+				//XXX: TODO: Count faults.
+				so->sync_set_number = -1;
+				return;
+			}
+
+			so->did_handle_ootx = 1;
 		}
 
-		//printf( "%s / %d %d ++ %d %d\n", so->codename, dl, tpco, offset_from, acode );
+
+		if (acode > 3) {
+			if( ssn == 0 )
+			{
+				SV_INFO( "Warning: got a slave marker but only got a master sync." );
+			}
+			dl = so->last_time[1];
+			tpco = so->last_length[1];
+		}
 
 		int32_t offset_from = le->timestamp - dl + le->length/2;
 
 		//Make sure pulse is in valid window
 		if( offset_from < 380000 && offset_from > 70000 )
 		{
-			ct->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length );
+			ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length );
 		}
 	}
 	else
@@ -158,7 +180,6 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
 		//printf( "FAIL %d   %d - %d = %d\n", le->length, so->last_photo_time, le->timestamp, so->last_photo_time - le->timestamp );
 		//Runt pulse, or no sync pulses available.
 	}
-#endif
 }