From 3ec21bdcad82968bbed5ee1035d9ebedff7c67cf Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Thu, 22 Mar 2018 08:32:04 -0600 Subject: Split out disambiguators into seperate compilation units --- src/survive_charlesbiguator.c | 193 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 193 insertions(+) create mode 100644 src/survive_charlesbiguator.c (limited to 'src/survive_charlesbiguator.c') 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 /* for sqrt */ +#include +#include + +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. + } +} -- cgit v1.2.3