//<>< (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 const float tau_table[33] = {0,
0,
0,
1.151140982,
1.425,
1.5712213707,
1.656266074,
1.7110275587,
1.7490784054,
1.7770229476,
1.798410005,
1.8153056661,
1.8289916275,
1.8403044103,
1.8498129961,
1.8579178211,
1.864908883,
1.8710013691,
1.8763583296,
1.881105575,
1.885341741,
1.8891452542,
1.8925792599,
1.8956951735,
1.8985352854,
1.9011347009,
1.9035228046,
1.9057243816,
1.9077604832,
1.9096491058,
1.9114057255,
1.9130437248,
1.914574735};
typedef struct {
unsigned int sweep_time[SENSORS_PER_OBJECT];
uint16_t sweep_len[SENSORS_PER_OBJECT]; // might want to align this to cache lines, will be hot for frequent access
} lightcaps_sweep_data;
typedef struct {
int recent_sync_time;
int activeLighthouse;
int activeSweepStartTime;
int activeAcode;
// int lh_pulse_len[NUM_LIGHTHOUSES];
int lh_start_time[NUM_LIGHTHOUSES];
int lh_max_pulse_length[NUM_LIGHTHOUSES];
int8_t lh_acode[NUM_LIGHTHOUSES];
int current_lh; // used knowing which sync pulse we're looking at.
} lightcap2_per_sweep_data;
typedef struct {
double acode_offset;
int sent_out_ootx_bits;
} lightcap2_global_data;
typedef struct {
lightcaps_sweep_data sweep;
lightcap2_per_sweep_data per_sweep;
lightcap2_global_data global;
} lightcap2_data;
// static lightcap2_global_data lcgd = { 0 };
static int handle_lightcap2_getAcodeFromSyncPulse(SurviveObject *so, int pulseLen) {
double oldOffset = ((lightcap2_data *)so->disambiguator_data)->global.acode_offset;
int modifiedPulseLen = pulseLen - (int)oldOffset;
double newOffset = (((pulseLen) + 250) % 500) - 250;
((lightcap2_data *)so->disambiguator_data)->global.acode_offset = oldOffset * 0.9 + newOffset * 0.1;
// fprintf(stderr, " %f\n", oldOffset);
#define ACODE_OFFSET 0
if (pulseLen < 3250 - ACODE_OFFSET)
return 0;
if (pulseLen < 3750 - ACODE_OFFSET)
return 1;
if (pulseLen < 4250 - ACODE_OFFSET)
return 2;
if (pulseLen < 4750 - ACODE_OFFSET)
return 3;
if (pulseLen < 5250 - ACODE_OFFSET)
return 4;
if (pulseLen < 5750 - ACODE_OFFSET)
return 5;
if (pulseLen < 6250 - ACODE_OFFSET)
return 6;
return 7;
}
static uint8_t remove_outliers(SurviveObject *so) {
return 0; // disabling this for now because it seems remove almost all the points for wired watchman and wired
// tracker.
lightcap2_data *lcd = so->disambiguator_data;
uint32_t sum = 0;
uint8_t non_zero_count = 0;
uint32_t mean = 0;
uint16_t *min = NULL;
uint16_t *max = NULL;
uint8_t found_first = 0;
// future: https://gcc.gnu.org/projects/tree-ssa/vectorization.html#vectorizab
for (uint8_t i = 0; i < SENSORS_PER_OBJECT; i++) {
sum += lcd->sweep.sweep_len[i];
if (lcd->sweep.sweep_len[i] > 0)
++non_zero_count;
}
if (non_zero_count == 0)
return 0;
mean = sum / non_zero_count;
float standard_deviation = 0.0f;
sum = 0;
for (uint8_t i = 0; i < SENSORS_PER_OBJECT; i++) {
uint16_t len = lcd->sweep.sweep_len[i];
if (len > 0) {
sum += (len - mean) * (len - mean);
if (found_first == 0) {
max = min = lcd->sweep.sweep_len + i;
found_first = 1;
} else {
if (lcd->sweep.sweep_len[i] < *min)
min = lcd->sweep.sweep_len + i;
if (lcd->sweep.sweep_len[i] > *max)
max = lcd->sweep.sweep_len + i;
}
}
}
standard_deviation = sqrtf(((float)sum) / ((float)non_zero_count));
// printf("%f\n", standard_deviation);
float tau_test = standard_deviation;
if (non_zero_count > 2)
tau_test = standard_deviation * tau_table[non_zero_count];
// uint8_t removed_outliers = 0;
uint32_t d1 = mean - *min;
uint32_t d2 = *max - mean;
if (d1 > d2) {
if (d1 > tau_test) {
*min = 0;
return 1;
}
} else if (d2 > tau_test) {
*max = 0;
return 1;
}
return 0;
/*
for (uint8_t i = 0; i < SENSORS_PER_OBJECT; i++)
{
uint16_t len = lcd->sweep.sweep_len[i];
if (len == 0) continue;
if ( abs(len-mean) > tau_test )
{
// fprintf(stderr, "removing %d\n", len);
lcd->sweep.sweep_len[i] = 0;
removed_outliers = 1;
}
}
*/
// return removed_outliers;
}
static void handle_lightcap2_process_sweep_data(SurviveObject *so) {
lightcap2_data *lcd = so->disambiguator_data;
while (remove_outliers(so))
;
// look at all of the sensors we found, and process the ones that were hit.
// TODO: find the sensor(s) with the longest pulse length, and assume
// those are the "highest quality". Then, reject any pulses that are sufficiently
// different from those values, assuming that they are reflections.
{
unsigned int longest_pulse = 0;
unsigned int timestamp_of_longest_pulse = 0;
(void)timestamp_of_longest_pulse;
for (int i = 0; i < SENSORS_PER_OBJECT; i++) {
if (lcd->sweep.sweep_len[i] > longest_pulse) {
longest_pulse = lcd->sweep.sweep_len[i];
timestamp_of_longest_pulse = lcd->sweep.sweep_time[i];
}
}
int allZero = 1;
for (int q = 0; q < 32; q++)
if (lcd->sweep.sweep_len[q] != 0)
allZero = 0;
// if (!allZero)
// printf("a[%d]l[%d] ", lcd->per_sweep.activeAcode & 5, lcd->per_sweep.activeLighthouse);
for (int i = 0; i < SENSORS_PER_OBJECT; i++) {
{
static int counts[SENSORS_PER_OBJECT][2] = {0};
(void)counts;
// if (lcd->per_sweep.activeLighthouse == 0 && !allZero)
if (lcd->per_sweep.activeLighthouse > -1 && !allZero) {
if (lcd->sweep.sweep_len[i] != 0) {
// printf("%d ", i);
// counts[i][lcd->per_sweep.activeAcode & 1] ++;
} else {
counts[i][lcd->per_sweep.activeAcode & 1] = 0;
}
// if (counts[i][0] > 10 && counts[i][1] > 10)
//{
// printf("%d(%d,%d), ", i, counts[i][0], counts[i][1]);
//}
}
}
if (lcd->sweep.sweep_len[i] != 0) // if the sensor was hit, process it
{
// printf("%4d\n", lcd->sweep.sweep_len[i]);
int offset_from =
lcd->sweep.sweep_time[i] - lcd->per_sweep.activeSweepStartTime + lcd->sweep.sweep_len[i] / 2;
// first, send out the sync pulse data for the last round (for OOTX decoding
if (!lcd->global.sent_out_ootx_bits) {
if (lcd->per_sweep.lh_max_pulse_length[0] != 0) {
so->ctx->lightproc(
so, -1, handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_max_pulse_length[0]),
lcd->per_sweep.lh_max_pulse_length[0], lcd->per_sweep.lh_start_time[0], 0, 0);
}
if (lcd->per_sweep.lh_max_pulse_length[1] != 0) {
so->ctx->lightproc(
so, -2, handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_max_pulse_length[1]),
lcd->per_sweep.lh_max_pulse_length[1], lcd->per_sweep.lh_start_time[1], 0, 1);
}
lcd->global.sent_out_ootx_bits = 1;
}
// if (offset_from < 380000 && offset_from > 70000)
{
// if (longest_pulse *10 / 8 < lcd->sweep.sweep_len[i])
{
so->ctx->lightproc(so, i, lcd->per_sweep.activeAcode, offset_from, lcd->sweep.sweep_time[i],
lcd->sweep.sweep_len[i], lcd->per_sweep.activeLighthouse);
}
}
}
}
// if (!allZero)
// printf(" ..:..\n");
// if (!allZero) printf("\n");
}
// clear out sweep data (could probably limit this to only after a "first" sync.
// this is slightly more robust, so doing it here for now.
memset(&(((lightcap2_data *)so->disambiguator_data)->sweep), 0, sizeof(lightcaps_sweep_data));
}
static void handle_lightcap2_sync(SurviveObject *so, LightcapElement *le) {
// fprintf(stderr, "%6.6d %4.4d \n", le->timestamp - so->recent_sync_time, le->length);
lightcap2_data *lcd = so->disambiguator_data;
// static unsigned int recent_sync_time = 0;
// static unsigned int recent_sync_count = -1;
// static unsigned int activeSweepStartTime;
int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length); // acode for this sensor reading
// Process any sweep data we have
handle_lightcap2_process_sweep_data(so);
int time_since_last_sync = (le->timestamp - lcd->per_sweep.recent_sync_time);
// fprintf(stderr, " %2d %8d %d\n", le->sensor_id, time_since_last_sync, le->length);
// need to store up sync pulses, so we can take the earliest starting time for all sensors.
if (time_since_last_sync < 2400) {
lcd->per_sweep.recent_sync_time = le->timestamp;
// it's the same sync pulse;
// so->sync_set_number = 1;
so->recent_sync_time = le->timestamp;
// lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length;
// lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp;
if (le->length > lcd->per_sweep.lh_max_pulse_length[lcd->per_sweep.current_lh]) {
lcd->per_sweep.lh_max_pulse_length[lcd->per_sweep.current_lh] = le->length;
lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp;
lcd->per_sweep.lh_acode[lcd->per_sweep.current_lh] = acode;
}
/*
//this stuff should probably be happening on the sweep so that we can filter out erroneous a codes
if (!(acode >> 2 & 1)) // if the skip bit is not set
{
lcd->per_sweep.activeLighthouse = lcd->per_sweep.current_lh;
lcd->per_sweep.activeSweepStartTime = le->timestamp;
lcd->per_sweep.activeAcode = acode;
}
else
{
//this causes the master lighthouse to be ignored from the HMD
lcd->per_sweep.activeLighthouse = -1;
lcd->per_sweep.activeSweepStartTime = 0;
lcd->per_sweep.activeAcode = 0;
}
*/
} else if (time_since_last_sync < 24000) {
lcd->per_sweep.activeLighthouse = -1;
lcd->per_sweep.recent_sync_time = le->timestamp;
// I do believe we are lighthouse B
lcd->per_sweep.current_lh = 1;
// lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length;
lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp;
lcd->per_sweep.lh_max_pulse_length[lcd->per_sweep.current_lh] = le->length;
lcd->per_sweep.lh_acode[lcd->per_sweep.current_lh] = acode;
/*
if (!(acode >> 2 & 1)) // if the skip bit is not set
{
if (lcd->per_sweep.activeLighthouse != -1)
{
static int pulseWarningCount=0;
if (pulseWarningCount < 5)
{
pulseWarningCount++;
// hmm, it appears we got two non-skip pulses at the same time. That should never happen
fprintf(stderr, "WARNING: Two non-skip pulses received on the same cycle!\n");
}
}
lcd->per_sweep.activeLighthouse = 1;
lcd->per_sweep.activeSweepStartTime = le->timestamp;
lcd->per_sweep.activeAcode = acode;
}
*/
} else if (time_since_last_sync > 370000) {
// XXX CAUTION: if we lose sight of a lighthouse then, the remaining lighthouse will default to master
// this should probably be fixed. Maybe some kind of timing based guess at which lighthouse.
// looks like this is the first sync pulse. Cool!
lcd->global.sent_out_ootx_bits = 0;
// fprintf(stderr, "************************************ Reinitializing Disambiguator!!!\n");
// initialize here.
memset(&lcd->per_sweep, 0, sizeof(lcd->per_sweep));
lcd->per_sweep.activeLighthouse = -1;
for (uint8_t i = 0; i < NUM_LIGHTHOUSES; ++i) {
lcd->per_sweep.lh_acode[i] = -1;
}
lcd->per_sweep.recent_sync_time = le->timestamp;
// I do believe we are lighthouse A
lcd->per_sweep.current_lh = 0;
// lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length;
lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp;
lcd->per_sweep.lh_max_pulse_length[lcd->per_sweep.current_lh] = le->length;
lcd->per_sweep.lh_acode[lcd->per_sweep.current_lh] = acode;
// int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length);
/*
if (!(acode >> 2 & 1)) // if the skip bit is not set
{
lcd->per_sweep.activeLighthouse = 0;
lcd->per_sweep.activeSweepStartTime = le->timestamp;
lcd->per_sweep.activeAcode = acode;
}
*/
}
// printf("%d %d\n", acode, lcd->per_sweep.activeLighthouse );
}
static void handle_lightcap2_sweep(SurviveObject *so, LightcapElement *le) {
lightcap2_data *lcd = so->disambiguator_data;
// If we see multiple "hits" on the sweep for a given sensor,
// assume that the longest (i.e. strongest signal) is most likely
// the non-reflected signal.
// if (le->length < 80)
//{
// // this is a low-quality read. Better to throw it out than to use it.
// //fprintf(stderr, "%2d %d\n", le->sensor_id, le->length);
// return;
//}
// fprintf(stderr, "%2d %d\n", le->sensor_id, le->length);
// fprintf(stderr, ".");
lcd->per_sweep.activeLighthouse = -1;
lcd->per_sweep.activeSweepStartTime = 0;
lcd->per_sweep.activeAcode = 0;
for (uint8_t i = 0; i < NUM_LIGHTHOUSES; ++i) {
int acode = lcd->per_sweep.lh_acode[i];
if ((acode >= 0) && !(acode >> 2 & 1)) {
lcd->per_sweep.activeLighthouse = i;
lcd->per_sweep.activeSweepStartTime = lcd->per_sweep.lh_start_time[i];
lcd->per_sweep.activeAcode = acode;
}
}
if (lcd->per_sweep.activeLighthouse < 0) {
// fprintf(stderr, "WARNING: No active lighthouse!\n");
// fprintf(stderr, " %2d %8d %d %d\n", le->sensor_id,
// le->length,lcd->per_sweep.lh_acode[0],lcd->per_sweep.lh_acode[1]);
return;
}
if (lcd->sweep.sweep_len[le->sensor_id] < le->length) {
lcd->sweep.sweep_len[le->sensor_id] = le->length;
lcd->sweep.sweep_time[le->sensor_id] = le->timestamp;
}
}
void DisambiguatorTurvey(SurviveObject *so, LightcapElement *le) {
SurviveContext *ctx = so->ctx;
if (so->disambiguator_data == NULL) {
fprintf(stderr, "Initializing Disambiguator Data\n");
so->disambiguator_data = malloc(sizeof(lightcap2_data));
memset(so->disambiguator_data, 0, sizeof(lightcap2_data));
}
if (le->sensor_id > SENSORS_PER_OBJECT) {
return;
}
if (le->length > 6750) {
// Should never get a reading so high. Odd.
return;
}
// if (le->length >= 2750)
if (le->length >= 2500) {
// Looks like a sync pulse, process it!
handle_lightcap2_sync(so, le);
return;
}
// must be a sweep pulse, process it!
handle_lightcap2_sweep(so, le);
}
REGISTER_LINKTIME(DisambiguatorTurvey);
