Split out disambiguators into seperate compilation units

1 files changed, 193 insertions, 0 deletions

diff --git a/src/survive_charlesbiguator.c b/src/survive_charlesbiguator.c
new file mode 100644
index 0000000..8a4d71a
--- /dev/null
+++ b/src/survive_charlesbiguator.c
@@ -0,0 +1,193 @@
+//<>< (C) 2016 C. N. Lohr, MOSTLY Under MIT/x11 License.
+//
+#include "survive_internal.h"
+#include <math.h> /* for sqrt */
+#include <stdint.h>
+#include <string.h>
+
+static int32_t decode_acode(uint32_t length, int32_t main_divisor) {
+	//+50 adds a small offset and seems to help always get it right.
+	// Check the +50 in the future to see how well this works on a variety of hardware.
+	if (!main_divisor)
+		return -1;
+
+	int32_t acode = (length + main_divisor + 50) / (main_divisor * 2);
+	if (acode & 1)
+		return -1;
+
+	return (acode >> 1) - 6;
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+///////The charles disambiguator.  Don't use this, mostly here for
+///debugging.///////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+static void HandleOOTX(SurviveContext *ctx, SurviveObject *so) {
+	int32_t main_divisor = so->timebase_hz / 384000; // 125 @ 48 MHz.
+
+	int32_t acode_array[2] = {decode_acode(so->last_sync_length[0], main_divisor),
+							  decode_acode(so->last_sync_length[1], main_divisor)};
+
+	int32_t delta1 = so->last_sync_time[0] - so->recent_sync_time;
+	int32_t delta2 = so->last_sync_time[1] - so->last_sync_time[0];
+
+	// printf( "%p %p %d %d %d  %p\n", ctx, so, so->last_sync_time[0], acode_array, so->last_sync_length[0],
+	// ctx->lightproc );
+	if (acode_array[0] >= 0)
+		ctx->lightproc(so, -1, acode_array[0], delta1, so->last_sync_time[0], so->last_sync_length[0], 0);
+	if (acode_array[1] >= 0)
+		ctx->lightproc(so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1], 1);
+
+	so->recent_sync_time = so->last_sync_time[1];
+
+	so->did_handle_ootx = 1;
+}
+
+// This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode.
+void handle_lightcap_charlesbiguator(SurviveObject *so, LightcapElement *le) {
+	SurviveContext *ctx = so->ctx;
+	//	static int32_t last;
+	//	printf( "%d %lu %d %d\n", le->timestamp-last, le->timestamp, le->length, le->sensor_id );
+	//	last = le->timestamp;
+	// printf( "LE%3d%6d%12d\n", le->sensor_id, le->length, le->timestamp );
+
+	// int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time;
+
+	if (le->sensor_id > SENSORS_PER_OBJECT) {
+		return;
+	}
+
+	so->tsl = le->timestamp;
+	if (le->length < 20)
+		return; /// Assuming 20 is an okay value for here.
+
+	// 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; // lighthouse number
+	if (ssn < 0)
+		ssn = 0;
+#ifdef DEBUG
+	if (ssn >= NUM_LIGHTHOUSES) {
+		SV_INFO("ALGORITHMIC WARNING: ssn exceeds NUM_LIGHTHOUSES");
+	}
+#endif
+	int last_sync_time = so->last_sync_time[ssn];
+	int last_sync_length = so->last_sync_length[ssn];
+	int32_t delta = le->timestamp - last_sync_time; // Handle time wrapping (be sure to be int32)
+
+	if (delta < -so->pulsedist_max_ticks || delta > so->pulsedist_max_ticks) {
+		// Reset pulse, etc.
+		so->sync_set_number = -1;
+		delta = so->pulsedist_max_ticks;
+		//		return; //if we don't know what lighthouse this is we don't care to do much else
+	}
+
+	if (le->length > so->pulselength_min_sync) // Pulse longer indicates a sync pulse.
+	{
+		int is_new_pulse = delta > so->pulselength_min_sync /*1500*/ + last_sync_length;
+
+		if (is_new_pulse) {
+			int is_master_sync_pulse = delta > so->pulse_in_clear_time /*40000*/;
+			int is_pulse_from_same_lh_as_last_sweep;
+			int tp = delta % (so->timecenter_ticks * 2);
+			is_pulse_from_same_lh_as_last_sweep = tp < so->pulse_synctime_slack && tp > -so->pulse_synctime_slack;
+
+			if (!so->did_handle_ootx) {
+				HandleOOTX(ctx, so);
+			}
+			if (!is_master_sync_pulse) {
+				so->did_handle_ootx = 0;
+			}
+
+			if (is_master_sync_pulse) // Could also be called by slave if no master was seen.
+			{
+				ssn = so->sync_set_number = is_pulse_from_same_lh_as_last_sweep
+												? (so->sync_set_number)
+												: 0; // If repeated lighthouse, just back off one.
+				if (ssn < 0) {
+					SV_INFO("SEVERE WARNING: Pulse codes for tracking not able to be backed out.\n");
+					ssn = 0;
+				}
+				if (ssn != 0) {
+					// If it's the slave that is repeated, be sure to zero out its sync info.
+					so->last_sync_length[0] = 0;
+				} else {
+					so->last_sync_length[1] = 0;
+				}
+				so->last_sync_time[ssn] = le->timestamp;
+				so->last_sync_length[ssn] = le->length;
+			} else if (so->sync_set_number == -1) {
+				// Do nothing.
+			} else {
+				ssn = ++so->sync_set_number;
+
+				if (so->sync_set_number >= NUM_LIGHTHOUSES) {
+					SV_INFO("Warning.  Received an extra, unassociated sync pulse.");
+					ssn = so->sync_set_number = -1;
+				} else {
+					so->last_sync_time[ssn] = le->timestamp;
+					so->last_sync_length[ssn] = le->length;
+				}
+			}
+		} else {
+			// Find the longest pulse.
+			if (le->length > last_sync_length) {
+				if (so->last_sync_time[ssn] > le->timestamp) {
+					so->last_sync_time[ssn] = le->timestamp;
+					so->last_sync_length[ssn] = le->length;
+				}
+			}
+		}
+
+#if 0
+		//Extra tidbit for storing length-of-sync-pulses, if you want to try to use this to determine AoI or distance to LH.
+		//We don't actually use this anywhere, and I doubt we ever will?  Though, it could be useful at a later time to improve tracking.
+		{
+			int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz.
+			int base_station = is_new_pulse;
+			printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length ); //XXX sync_set_number is wrong here.
+			ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length, base_station); //XXX sync_set_number is wrong here.
+		}
+#endif
+	}
+
+	// Any else- statements below here are
+
+	// See if this is a valid actual pulse.
+	else if (le->length < so->pulse_max_for_sweep && delta > so->pulse_in_clear_time && ssn >= 0) {
+		int32_t tpco = so->last_sync_length[0];
+
+#if NUM_LIGHTHOUSES != 2
+#error You are going to have to fix the code around here to allow for something other than two base stations.
+#endif
+
+		// Adding length
+		// Long pulse-code from IR flood.
+		// Make sure it fits nicely into a divisible-by-500 time.
+
+		int32_t main_divisor = so->timebase_hz / 384000; // 125 @ 48 MHz.
+		int acode = decode_acode(so->last_sync_length[0], main_divisor);
+		int whichlh;
+		if (acode < 0)
+			whichlh = 1;
+		else
+			whichlh = (acode >> 2);
+		int32_t dl = so->last_sync_time[whichlh];
+
+		if (!so->did_handle_ootx)
+			HandleOOTX(ctx, so);
+
+		int32_t offset_from = le->timestamp - dl + le->length / 2;
+
+		// Make sure pulse is in valid window
+		if (offset_from < so->timecenter_ticks * 2 - so->pulse_in_clear_time && offset_from > so->pulse_in_clear_time) {
+			ctx->lightproc(so, le->sensor_id, acode, offset_from, le->timestamp, le->length, whichlh);
+		}
+	} else {
+		// 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.
+	}
+}