//<>< (C) 2016 C. N. Lohr, MOSTLY Under MIT/x11 License.
//
#include "survive_internal.h"
#include <assert.h>
#include <math.h> /* for sqrt */
#include <stdint.h>
#include <string.h>
#define PULSELENGTH_MIN_SYNC 2200
#define TIMECENTER_TICKS (48000000/240)
#define PULSEDIST_MAX_TICKS 500000
#define PULSE_IN_CLEAR_TIME 35000
#define PULSE_MAX_FOR_SWEEP 1800
#define PULSE_SYNCTIME_OFFSET 20000 //unused?
#define PULSE_SYNCTIME_SLACK 5000
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;
int32_t rtn = (acode >> 1) - 6;
if (rtn > 7 || rtn < 0) {
return -1;
}
return rtn;
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////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 DisambiguatorCharles(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 < -PULSEDIST_MAX_TICKS || delta > PULSEDIST_MAX_TICKS) {
// Reset pulse, etc.
so->sync_set_number = -1;
delta = PULSEDIST_MAX_TICKS;
// return; //if we don't know what lighthouse this is we don't care to do much else
}
if (le->length > PULSELENGTH_MIN_SYNC) // Pulse longer indicates a sync pulse.
{
int is_new_pulse = delta > PULSELENGTH_MIN_SYNC /*1500*/ + last_sync_length;
if (is_new_pulse) {
int is_master_sync_pulse = delta > PULSE_IN_CLEAR_TIME /*40000*/;
int is_pulse_from_same_lh_as_last_sweep;
int tp = delta % (TIMECENTER_TICKS * 2);
is_pulse_from_same_lh_as_last_sweep = tp < PULSE_SYNCTIME_SLACK && tp > -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 < PULSE_MAX_FOR_SWEEP && delta > 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);
// If acode isn't right; don't even think of emitting an event
if (acode >= 0) {
int whichlh = (acode >> 2);
assert(whichlh <= 1);
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 < TIMECENTER_TICKS * 2 - PULSE_IN_CLEAR_TIME &&
offset_from > 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.
}
}
REGISTER_LINKTIME(DisambiguatorCharles);
