From f3877f7b13c4f19ac101a0c6c7d4122a46db9a76 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Mon, 26 Mar 2018 12:35:42 -0600 Subject: Initial pass --- src/survive_tb_disambiguator.c | 143 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 143 insertions(+) create mode 100644 src/survive_tb_disambiguator.c (limited to 'src') diff --git a/src/survive_tb_disambiguator.c b/src/survive_tb_disambiguator.c new file mode 100644 index 0000000..5d79429 --- /dev/null +++ b/src/survive_tb_disambiguator.c @@ -0,0 +1,143 @@ +// +#include "survive_internal.h" +#include +#include /* for sqrt */ +#include +#include +#include + +#define NUM_HISTORY 3 + +enum LightcapClassification { LCC_UNKNOWN = 0, LCC_SYNC = 1, LCC_SWEEP = 2 }; + +typedef struct { + LightcapElement history[NUM_HISTORY]; + enum LightcapClassification classifications[NUM_HISTORY]; + int idx; +} SensorHistory_t; + +typedef struct { + LightcapElement last_sync; + SensorHistory_t histories[]; +} Disambiguator_data_t; + +Disambiguator_data_t *Disambiguator_data_t_ctor(SurviveObject *so) { + return calloc(sizeof(Disambiguator_data_t) + sizeof(SensorHistory_t) * so->sensor_ct, 1); +} + +static uint32_t timestamp_diff(uint32_t recent, uint32_t prior) { + if (recent > prior) + return recent - prior; + return (0xFFFFFFFF - prior) + recent; +} + +#define LOWER_SYNC_TIME 2250 +#define UPPER_SYNC_TIME 6750 + +static int circle_buffer_get(int idx, int offset) { return ((idx + offset) + NUM_HISTORY) % NUM_HISTORY; } + +static enum LightcapClassification classify(Disambiguator_data_t *d, SensorHistory_t *history, + const LightcapElement *le) { + bool clearlyNotSync = le->length < LOWER_SYNC_TIME || le->length > UPPER_SYNC_TIME; + + if (clearlyNotSync) { + return LCC_SWEEP; + } + + uint32_t time_diff_last_sync = timestamp_diff(le->timestamp, d->last_sync.timestamp); + uint32_t split_time = 399840; // 8.33ms in 48mhz + uint32_t jitter_allowance = 4000; + + // If we are ~8.33ms ahead of the last sync; we are a sync + if (d->last_sync.length > 0 && time_diff_last_sync < (split_time + jitter_allowance) && + time_diff_last_sync > (split_time - jitter_allowance)) { + return LCC_SYNC; + } + + if (d->last_sync.length > 0 && time_diff_last_sync < (split_time - jitter_allowance)) { + return LCC_SWEEP; + } + + int prevIdx = circle_buffer_get(history->idx, -1); + uint32_t time_diff = timestamp_diff(le->timestamp, history->history[prevIdx].timestamp); + + // We don't have recent data; unclear + if (time_diff > split_time - jitter_allowance) { + fprintf(stderr, "Time diff too high %d\n", time_diff); + return LCC_UNKNOWN; + } + + switch (history->classifications[prevIdx]) { + case LCC_SWEEP: + return LCC_SYNC; + } + fprintf(stderr, "last not sweep\n"); + return LCC_UNKNOWN; +} + +static enum LightcapClassification update_histories(Disambiguator_data_t *d, const LightcapElement *le) { + SensorHistory_t *history = &d->histories[le->sensor_id]; + + enum LightcapClassification classification = classify(d, history, le); + + history->classifications[history->idx] = classification; + history->history[history->idx] = *le; + history->idx = (history->idx + 1) % NUM_HISTORY; + + return classification; +} + +static int find_acode(uint32_t pulseLen) { + const static int offset = 50; + if (pulseLen < 2500 - offset) + return -1; + + if (pulseLen < 3000 + offset) + return 0; + if (pulseLen < 3500 + offset) + return 1; + if (pulseLen < 4000 + offset) + return 2; + if (pulseLen < 4500 + offset) + return 3; + if (pulseLen < 5000 + offset) + return 4; + if (pulseLen < 5500 + offset) + return 5; + if (pulseLen < 6000 + offset) + return 6; + if (pulseLen < 6500 + offset) + return 7; + + return -1; +} + +void DisambiguatorTimeBased(SurviveObject *so, const LightcapElement *le) { + SurviveContext *ctx = so->ctx; + if (so->disambiguator_data == NULL) { + SV_INFO("Initializing Disambiguator Data"); + so->disambiguator_data = Disambiguator_data_t_ctor(so); + } + + Disambiguator_data_t *d = so->disambiguator_data; + + SensorHistory_t *history = &d->histories[le->sensor_id]; + int prevIdx = circle_buffer_get(history->idx, -1); + uint32_t time_diff = timestamp_diff(le->timestamp, history->history[prevIdx].timestamp); + enum LightcapClassification classification = update_histories(d, le); + + SV_INFO("Classification %d\t%d\t%d\t%u\t%u(%.02fms)", classification, le->sensor_id, le->length, le->timestamp, + time_diff, (double)time_diff / 48000.0); + if (classification == LCC_SYNC) { + uint32_t time_diff_last_sync = timestamp_diff(le->timestamp, d->last_sync.timestamp); + if (time_diff_last_sync > (0xFFFFFFFF / 2)) + time_diff_last_sync = 0xFFFFFFFFF - time_diff_last_sync; + d->last_sync = *le; + int acode = find_acode(le->length); + SV_INFO("acode: %d 0x%x a:%d d:%d s:%d (%.02fms)", le->length, acode, acode & 1, (bool)(acode & 2), + (bool)(acode & 4), (double)time_diff_last_sync / 48000.); + assert(acode != -1); + } +} + +REGISTER_LINKTIME(DisambiguatorTimeBased); -- cgit v1.2.3 From 3963b2bfe6954bf647d112b2a435910baa5529b2 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Mon, 26 Mar 2018 19:22:54 -0600 Subject: Fixed text in menu text --- src/survive.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/survive.c b/src/survive.c index 73d6474..899d206 100644 --- a/src/survive.c +++ b/src/survive.c @@ -219,7 +219,7 @@ void *GetDriverByConfig(SurviveContext *ctx, const char *name, const char *confi int prefixLen = strlen(name); if (verbose > 1) - SV_INFO("Available %s:", name); + SV_INFO("Available %ss:", name); while ((DriverName = GetDriverNameMatching(name, i++))) { void *p = GetDriver(DriverName); @@ -237,7 +237,7 @@ void *GetDriverByConfig(SurviveContext *ctx, const char *name, const char *confi if (verbose > 1) SV_INFO("Totals %d %ss.", i - 1, name); if (verbose > 0) - SV_INFO("Using %s for %s", name, configname); + SV_INFO("Using '%s' for %s", picked, configname); return func; } -- cgit v1.2.3 From 0d10ca10f10a7c16828ae88ebd9a55fde5938efe Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Mon, 26 Mar 2018 23:20:31 -0600 Subject: Added notes before major overhaul --- src/survive_tb_disambiguator.c | 219 ++++++++++++++++++++++++++++++++--------- src/survive_vive.c | 10 -- 2 files changed, 173 insertions(+), 56 deletions(-) (limited to 'src') diff --git a/src/survive_tb_disambiguator.c b/src/survive_tb_disambiguator.c index 5d79429..b4dd0ea 100644 --- a/src/survive_tb_disambiguator.c +++ b/src/survive_tb_disambiguator.c @@ -8,6 +8,46 @@ #define NUM_HISTORY 3 +/** + * The lighthouses go in the following order: + * + * Ticks State + * 0 ACode 0b1x0 (4) + * 20 000 ACode 0b0x0 (0) + * LH A X Sweep + * 400 000 ACode 0b1x1 (5) + * 420 000 ACode 0b0x1 (1) + * LH A Y SWEEP + * 800 000 ACode 0b0x0 (0) + * 820 000 ACode 0b1x0 (4) + * LH B X Sweep + * 1 200 000 ACode 0b0x1 (1) + * 1 220 000 ACode 0b1x1 (5) + * LH B Y SWEEP + * 1 600 000 < REPEAT > + * + * NOTE: Obviously you cut the data bit out for this + */ + +enum LighthouseState { + LS_UNKNOWN = 0 + + LS_WaitLHA_ACode4 = 1, + LS_WaitLHA_ACode0, + LS_SweepAX, + LS_WaitLHA_ACode5, + LS_WaitLHA_ACode1, + LS_SweepAY, + LS_WaitLHB_ACode0, + LS_WaitLHB_ACode4, + LS_SweepBX, + LS_WaitLHB_ACode1, + LS_WaitLHB_ACode5, + LS_SweepBY, + + LS_END +}; + enum LightcapClassification { LCC_UNKNOWN = 0, LCC_SYNC = 1, LCC_SWEEP = 2 }; typedef struct { @@ -17,12 +57,19 @@ typedef struct { } SensorHistory_t; typedef struct { - LightcapElement last_sync; + uint64_t last_sync_timestamp; + uint64_t last_sync_length; + int last_sync_count; + bool lastWasSync; + + uint32_t mod_offset; + LighthouseState state; + SensorHistory_t histories[]; } Disambiguator_data_t; Disambiguator_data_t *Disambiguator_data_t_ctor(SurviveObject *so) { - return calloc(sizeof(Disambiguator_data_t) + sizeof(SensorHistory_t) * so->sensor_ct, 1); + return calloc(1, sizeof(Disambiguator_data_t) + sizeof(SensorHistory_t) * so->sensor_ct); } static uint32_t timestamp_diff(uint32_t recent, uint32_t prior) { @@ -31,11 +78,67 @@ static uint32_t timestamp_diff(uint32_t recent, uint32_t prior) { return (0xFFFFFFFF - prior) + recent; } +static int find_acode(uint32_t pulseLen) { + const static int offset = 0; + if (pulseLen < 2200 + offset) + return -1; + + if (pulseLen < 3000 + offset) + return 0; + if (pulseLen < 3500 + offset) + return 1; + if (pulseLen < 4000 + offset) + return 2; + if (pulseLen < 4500 + offset) + return 3; + if (pulseLen < 5000 + offset) + return 4; + if (pulseLen < 5500 + offset) + return 5; + if (pulseLen < 6000 + offset) + return 6; + if (pulseLen < 6500 + offset) + return 7; + + return -1; +} + #define LOWER_SYNC_TIME 2250 #define UPPER_SYNC_TIME 6750 static int circle_buffer_get(int idx, int offset) { return ((idx + offset) + NUM_HISTORY) % NUM_HISTORY; } +static bool overlaps(const LightcapElement *a, const LightcapElement *b) { + int overlap = 0; + if (a->timestamp < b->timestamp && a->length + a->timestamp > b->timestamp) + overlap = a->length + a->timestamp - b->timestamp; + else if (b->timestamp < a->timestamp && b->length + b->timestamp > a->timestamp) + overlap = b->length + b->timestamp - a->timestamp; + + return overlap > a->length / 2; +} + +const int SKIP_BIT = 4; +const int DATA_BIT = 2; +const int AXIS_BIT = 1; + +LightcapElement get_last_sync(Disambiguator_data_t *d) { + if (d->last_sync_count == 0) { + return (LightcapElement){0}; + } + + return (LightcapElement){.timestamp = (d->last_sync_timestamp + d->last_sync_count / 2) / d->last_sync_count, + .length = (d->last_sync_length + d->last_sync_count / 2) / d->last_sync_count, + .sensor_id = -d->last_sync_count}; +} + +static uint32_t next_sync_expected(Disambiguator_data_t *d) { + int acode = find_acode(get_last_sync(d).length); + if (acode & SKIP_BIT) + return get_last_sync(d).timestamp + 20000; + return get_last_sync(d).timestamp + 399840; +} + static enum LightcapClassification classify(Disambiguator_data_t *d, SensorHistory_t *history, const LightcapElement *le) { bool clearlyNotSync = le->length < LOWER_SYNC_TIME || le->length > UPPER_SYNC_TIME; @@ -44,17 +147,24 @@ static enum LightcapClassification classify(Disambiguator_data_t *d, SensorHisto return LCC_SWEEP; } - uint32_t time_diff_last_sync = timestamp_diff(le->timestamp, d->last_sync.timestamp); + uint32_t time_diff_last_sync = timestamp_diff(le->timestamp, get_last_sync(d).timestamp); uint32_t split_time = 399840; // 8.33ms in 48mhz - uint32_t jitter_allowance = 4000; + uint32_t jitter_allowance = 20000; // If we are ~8.33ms ahead of the last sync; we are a sync - if (d->last_sync.length > 0 && time_diff_last_sync < (split_time + jitter_allowance) && - time_diff_last_sync > (split_time - jitter_allowance)) { + if (get_last_sync(d).length > 0 && abs(timestamp_diff(le->timestamp, next_sync_expected(d))) < jitter_allowance) { return LCC_SYNC; } - if (d->last_sync.length > 0 && time_diff_last_sync < (split_time - jitter_allowance)) { + LightcapElement last_sync = get_last_sync(d); + if (get_last_sync(d).length > 0 && overlaps(&last_sync, le)) { + return LCC_SYNC; + } + + if (le->length > 2000) + return LCC_SYNC; + + if (get_last_sync(d).length > 0 && time_diff_last_sync < (split_time - jitter_allowance)) { return LCC_SWEEP; } @@ -62,7 +172,7 @@ static enum LightcapClassification classify(Disambiguator_data_t *d, SensorHisto uint32_t time_diff = timestamp_diff(le->timestamp, history->history[prevIdx].timestamp); // We don't have recent data; unclear - if (time_diff > split_time - jitter_allowance) { + if (time_diff > split_time) { fprintf(stderr, "Time diff too high %d\n", time_diff); return LCC_UNKNOWN; } @@ -79,43 +189,22 @@ static enum LightcapClassification update_histories(Disambiguator_data_t *d, con SensorHistory_t *history = &d->histories[le->sensor_id]; enum LightcapClassification classification = classify(d, history, le); - history->classifications[history->idx] = classification; history->history[history->idx] = *le; history->idx = (history->idx + 1) % NUM_HISTORY; - return classification; } -static int find_acode(uint32_t pulseLen) { - const static int offset = 50; - if (pulseLen < 2500 - offset) - return -1; - - if (pulseLen < 3000 + offset) - return 0; - if (pulseLen < 3500 + offset) - return 1; - if (pulseLen < 4000 + offset) - return 2; - if (pulseLen < 4500 + offset) - return 3; - if (pulseLen < 5000 + offset) - return 4; - if (pulseLen < 5500 + offset) - return 5; - if (pulseLen < 6000 + offset) - return 6; - if (pulseLen < 6500 + offset) - return 7; - - return -1; -} - void DisambiguatorTimeBased(SurviveObject *so, const LightcapElement *le) { SurviveContext *ctx = so->ctx; + + // Note, this happens if we don't have config yet -- just bail + if (so->sensor_ct == 0) { + return; + } + if (so->disambiguator_data == NULL) { - SV_INFO("Initializing Disambiguator Data"); + SV_INFO("Initializing Disambiguator Data for TB %d", so->sensor_ct); so->disambiguator_data = Disambiguator_data_t_ctor(so); } @@ -126,18 +215,56 @@ void DisambiguatorTimeBased(SurviveObject *so, const LightcapElement *le) { uint32_t time_diff = timestamp_diff(le->timestamp, history->history[prevIdx].timestamp); enum LightcapClassification classification = update_histories(d, le); - SV_INFO("Classification %d\t%d\t%d\t%u\t%u(%.02fms)", classification, le->sensor_id, le->length, le->timestamp, - time_diff, (double)time_diff / 48000.0); + uint32_t time_diff_last_sync = timestamp_diff(le->timestamp, get_last_sync(d).timestamp); + if (time_diff_last_sync > (0xFFFFFFFF / 2)) + time_diff_last_sync = 0xFFFFFFFFF - time_diff_last_sync; + + LightcapElement lastSync = get_last_sync(d); + int acode = find_acode(lastSync.length); + if (classification == LCC_SYNC) { - uint32_t time_diff_last_sync = timestamp_diff(le->timestamp, d->last_sync.timestamp); - if (time_diff_last_sync > (0xFFFFFFFF / 2)) - time_diff_last_sync = 0xFFFFFFFFF - time_diff_last_sync; - d->last_sync = *le; - int acode = find_acode(le->length); - SV_INFO("acode: %d 0x%x a:%d d:%d s:%d (%.02fms)", le->length, acode, acode & 1, (bool)(acode & 2), - (bool)(acode & 4), (double)time_diff_last_sync / 48000.); - assert(acode != -1); + LightcapElement lastSync = get_last_sync(d); + if (d->lastWasSync == false || overlaps(&lastSync, le) == false) { + + if (lastSync.length) { + int acode = find_acode(lastSync.length); + SV_INFO("%.03f(%d)\tacode: %d 0x%x a:%d d:%d s:%d (%d)", + timestamp_diff(le->timestamp, lastSync.timestamp) / 48000., + timestamp_diff(le->timestamp, lastSync.timestamp), lastSync.length, acode, acode & 1, + (bool)(acode & 2), (bool)(acode & 4), ((acode >> 1) & 0x2) | (acode & 1)); + assert(acode != -1); + } + d->last_sync_timestamp = le->timestamp; + d->last_sync_length = le->length; + d->last_sync_count = 1; + } else { + d->last_sync_timestamp += le->timestamp; + d->last_sync_length += le->length; + d->last_sync_count++; + } + + d->lastWasSync = true; + + lastSync = get_last_sync(d); + // SV_INFO("acode building: %u %u %u", lastSync.length, lastSync.timestamp, lastSync.length + + // lastSync.timestamp); + } else { + if (d->lastWasSync) { + if (lastSync.length) { + int acode = find_acode(lastSync.length); + SV_INFO("start acode: %d 0x%x a:%d d:%d s:%d (%d)", lastSync.length, acode, acode & 1, + (bool)(acode & 2), (bool)(acode & 4), ((acode >> 1) & 0x2) | (acode & 1)); + assert(acode != -1); + } + } + d->lastWasSync = false; } + + if (classification == LCC_SWEEP) + SV_INFO("%.02fms Classification %d\t%d\t%d\t%u\t%u\t(%.02fms)\ttime since last sync: %.02fms a:%d d:%d s:%d", + (double)le->timestamp / 48000, classification, le->sensor_id, le->length, le->timestamp, time_diff, + (double)time_diff / 48000.0, (double)time_diff_last_sync / 48000., acode & 1, (bool)(acode & 2), + (bool)(acode & 4)); } REGISTER_LINKTIME(DisambiguatorTimeBased); diff --git a/src/survive_vive.c b/src/survive_vive.c index 91f25af..3c60b2a 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -1708,16 +1708,6 @@ int survive_vive_close( SurviveContext * ctx, void * driver ) return 0; } -void init_SurviveObject(SurviveObject* so) { - so->acc_scale = NULL; - so->acc_bias = NULL; - so->gyro_scale = NULL; - so->gyro_bias = NULL; - so->haptic = NULL; - so->PoserData = NULL; - so->disambiguator_data = NULL; -} - int DriverRegHTCVive( SurviveContext * ctx ) { const char *playback_dir = survive_configs(ctx, "playback", SC_GET, ""); -- cgit v1.2.3 From 0a4722c50ddfe12e59aba9c79ec1988f9b82f997 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Tue, 27 Mar 2018 00:16:13 -0600 Subject: Find state works well --- src/survive_tb_disambiguator.c | 166 ++++++++++++++++++++++++++++------------- 1 file changed, 114 insertions(+), 52 deletions(-) (limited to 'src') diff --git a/src/survive_tb_disambiguator.c b/src/survive_tb_disambiguator.c index b4dd0ea..dda55ff 100644 --- a/src/survive_tb_disambiguator.c +++ b/src/survive_tb_disambiguator.c @@ -30,7 +30,7 @@ */ enum LighthouseState { - LS_UNKNOWN = 0 + LS_UNKNOWN = 0, LS_WaitLHA_ACode4 = 1, LS_WaitLHA_ACode0, @@ -48,6 +48,40 @@ enum LighthouseState { LS_END }; +int LighthouseState_offset(enum LighthouseState s) { + int mini_jump = 20000; + int big_jump = 360000; + switch (s) { + case LS_WaitLHA_ACode4: + return 0; + case LS_WaitLHA_ACode0: + return mini_jump; + case LS_SweepAX: + return 2 * mini_jump; + case LS_WaitLHA_ACode5: + return 2 * mini_jump + big_jump; + case LS_WaitLHA_ACode1: + return 3 * mini_jump + big_jump; + case LS_SweepAY: + return 4 * mini_jump + big_jump; + case LS_WaitLHB_ACode0: + return 4 * mini_jump + 2 * big_jump; + case LS_WaitLHB_ACode4: + return 5 * mini_jump + 2 * big_jump; + case LS_SweepBX: + return 6 * mini_jump + 2 * big_jump; + case LS_WaitLHB_ACode1: + return 6 * mini_jump + 3 * big_jump; + case LS_WaitLHB_ACode5: + return 7 * mini_jump + 3 * big_jump; + case LS_SweepBY: + return 8 * mini_jump + 3 * big_jump; + case LS_END: + return 8 * mini_jump + 4 * big_jump; + } + return -1; +} + enum LightcapClassification { LCC_UNKNOWN = 0, LCC_SYNC = 1, LCC_SWEEP = 2 }; typedef struct { @@ -57,19 +91,30 @@ typedef struct { } SensorHistory_t; typedef struct { + SurviveObject *so; + /** This part of the structure is general use when we know our state */ + uint32_t mod_offset; + enum LighthouseState state; + int confidence; + + /** This rest of the structure is dedicated to finding a state when we are unknown */ + + int encoded_acodes; + /* Keep running average of sync signals as they come in */ uint64_t last_sync_timestamp; uint64_t last_sync_length; int last_sync_count; - bool lastWasSync; - - uint32_t mod_offset; - LighthouseState state; + bool lastWasSync; SensorHistory_t histories[]; + } Disambiguator_data_t; Disambiguator_data_t *Disambiguator_data_t_ctor(SurviveObject *so) { - return calloc(1, sizeof(Disambiguator_data_t) + sizeof(SensorHistory_t) * so->sensor_ct); + Disambiguator_data_t *rtn = calloc(1, sizeof(Disambiguator_data_t) + sizeof(SensorHistory_t) * so->sensor_ct); + rtn->so = so; + + return rtn; } static uint32_t timestamp_diff(uint32_t recent, uint32_t prior) { @@ -195,45 +240,48 @@ static enum LightcapClassification update_histories(Disambiguator_data_t *d, con return classification; } -void DisambiguatorTimeBased(SurviveObject *so, const LightcapElement *le) { - SurviveContext *ctx = so->ctx; - - // Note, this happens if we don't have config yet -- just bail - if (so->sensor_ct == 0) { - return; - } +static enum LighthouseState EndSync(Disambiguator_data_t *d, const LightcapElement *le) { + SurviveContext *ctx = d->so->ctx; + LightcapElement lastSync = get_last_sync(d); + int acode = find_acode(lastSync.length); + SV_INFO("!!%.03f(%d)\tacode: %d 0x%x a:%d d:%d s:%d (%d)", + timestamp_diff(le->timestamp, lastSync.timestamp) / 48000., + timestamp_diff(le->timestamp, lastSync.timestamp), lastSync.length, acode, acode & 1, (bool)(acode & 2), + (bool)(acode & 4), acode & (SKIP_BIT | AXIS_BIT)); + + if (acode > 0) { + d->encoded_acodes &= 0xFFFF; + d->encoded_acodes = (d->encoded_acodes << 8) | (acode & (SKIP_BIT | AXIS_BIT)); + SV_INFO("%x", d->encoded_acodes); + switch (d->encoded_acodes) { + case (5 << 16) | (4 << 8) | 0: + return d->state = LS_SweepAX - 1; + case (0 << 16) | (5 << 8) | 1: + return d->state = LS_SweepAY - 1; + case (1 << 16) | (0 << 8) | 4: + return d->state = LS_SweepBX - 1; + case (4 << 16) | (1 << 8) | 5: + return d->state = LS_SweepBY - 1; + } - if (so->disambiguator_data == NULL) { - SV_INFO("Initializing Disambiguator Data for TB %d", so->sensor_ct); - so->disambiguator_data = Disambiguator_data_t_ctor(so); + } else { + d->encoded_acodes = 0; } - Disambiguator_data_t *d = so->disambiguator_data; + return LS_UNKNOWN; +} - SensorHistory_t *history = &d->histories[le->sensor_id]; - int prevIdx = circle_buffer_get(history->idx, -1); - uint32_t time_diff = timestamp_diff(le->timestamp, history->history[prevIdx].timestamp); +static enum LighthouseState AttemptFindState(Disambiguator_data_t *d, const LightcapElement *le) { enum LightcapClassification classification = update_histories(d, le); - uint32_t time_diff_last_sync = timestamp_diff(le->timestamp, get_last_sync(d).timestamp); - if (time_diff_last_sync > (0xFFFFFFFF / 2)) - time_diff_last_sync = 0xFFFFFFFFF - time_diff_last_sync; - - LightcapElement lastSync = get_last_sync(d); - int acode = find_acode(lastSync.length); - if (classification == LCC_SYNC) { LightcapElement lastSync = get_last_sync(d); + if (d->lastWasSync == false || overlaps(&lastSync, le) == false) { + enum LighthouseState new_state = EndSync(d, le); + if (new_state != LS_UNKNOWN) + return new_state; - if (lastSync.length) { - int acode = find_acode(lastSync.length); - SV_INFO("%.03f(%d)\tacode: %d 0x%x a:%d d:%d s:%d (%d)", - timestamp_diff(le->timestamp, lastSync.timestamp) / 48000., - timestamp_diff(le->timestamp, lastSync.timestamp), lastSync.length, acode, acode & 1, - (bool)(acode & 2), (bool)(acode & 4), ((acode >> 1) & 0x2) | (acode & 1)); - assert(acode != -1); - } d->last_sync_timestamp = le->timestamp; d->last_sync_length = le->length; d->last_sync_count = 1; @@ -244,27 +292,41 @@ void DisambiguatorTimeBased(SurviveObject *so, const LightcapElement *le) { } d->lastWasSync = true; - - lastSync = get_last_sync(d); - // SV_INFO("acode building: %u %u %u", lastSync.length, lastSync.timestamp, lastSync.length + - // lastSync.timestamp); } else { - if (d->lastWasSync) { - if (lastSync.length) { - int acode = find_acode(lastSync.length); - SV_INFO("start acode: %d 0x%x a:%d d:%d s:%d (%d)", lastSync.length, acode, acode & 1, - (bool)(acode & 2), (bool)(acode & 4), ((acode >> 1) & 0x2) | (acode & 1)); - assert(acode != -1); - } - } d->lastWasSync = false; } - if (classification == LCC_SWEEP) - SV_INFO("%.02fms Classification %d\t%d\t%d\t%u\t%u\t(%.02fms)\ttime since last sync: %.02fms a:%d d:%d s:%d", - (double)le->timestamp / 48000, classification, le->sensor_id, le->length, le->timestamp, time_diff, - (double)time_diff / 48000.0, (double)time_diff_last_sync / 48000., acode & 1, (bool)(acode & 2), - (bool)(acode & 4)); + return LS_UNKNOWN; +} + +void PropagateState(Disambiguator_data_t *d, const LightcapElement *le) { d->state = LS_UNKNOWN; } + +void DisambiguatorTimeBased(SurviveObject *so, const LightcapElement *le) { + SurviveContext *ctx = so->ctx; + + // Note, this happens if we don't have config yet -- just bail + if (so->sensor_ct == 0) { + return; + } + + if (so->disambiguator_data == NULL) { + SV_INFO("Initializing Disambiguator Data for TB %d", so->sensor_ct); + so->disambiguator_data = Disambiguator_data_t_ctor(so); + } + + Disambiguator_data_t *d = so->disambiguator_data; + + if (d->state == LS_UNKNOWN) { + enum LighthouseState new_state = AttemptFindState(d, le); + if (new_state != LS_UNKNOWN) { + d->confidence = 0; + d->mod_offset = (le->timestamp % LighthouseState_offset(LS_END)) - LighthouseState_offset(new_state); + d->state = new_state; + SV_INFO("Locked onto state %d at %d", new_state, d->mod_offset); + } + } else { + PropagateState(d, le); + } } REGISTER_LINKTIME(DisambiguatorTimeBased); -- cgit v1.2.3 From 0ff8f72a30a4c659815696074f672569d02e7718 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Tue, 27 Mar 2018 08:15:39 -0600 Subject: State machine sorta works; but times are backwards?! --- src/survive_tb_disambiguator.c | 112 +++++++++++++++++++++++++++++++++++++++-- 1 file changed, 108 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/survive_tb_disambiguator.c b/src/survive_tb_disambiguator.c index dda55ff..e09d3da 100644 --- a/src/survive_tb_disambiguator.c +++ b/src/survive_tb_disambiguator.c @@ -29,6 +29,10 @@ * NOTE: Obviously you cut the data bit out for this */ +// Every pulse_window seems roughly 20k ticks long. That leaves ~360 to the capture window +#define PULSE_WINDOW 20000 +#define CAPTURE_WINDOW 360000 + enum LighthouseState { LS_UNKNOWN = 0, @@ -48,6 +52,63 @@ enum LighthouseState { LS_END }; +void LighthouseState_Parameterize(enum LighthouseState s, int *acode, int *lh, int *axis, int *window) { + *lh = *axis = *acode = -1; + switch (s) { + case LS_WaitLHB_ACode4: + case LS_WaitLHA_ACode4: + case LS_WaitLHB_ACode0: + case LS_WaitLHA_ACode0: + case LS_WaitLHB_ACode5: + case LS_WaitLHA_ACode5: + case LS_WaitLHB_ACode1: + case LS_WaitLHA_ACode1: + *window = PULSE_WINDOW; + break; + case LS_SweepAX: + case LS_SweepAY: + case LS_SweepBX: + case LS_SweepBY: + *window = CAPTURE_WINDOW; + break; + } + + switch (s) { + case LS_WaitLHB_ACode4: + case LS_WaitLHA_ACode4: + *acode = 4; + break; + case LS_WaitLHB_ACode0: + case LS_WaitLHA_ACode0: + *acode = 0; + break; + case LS_WaitLHB_ACode5: + case LS_WaitLHA_ACode5: + *acode = 5; + break; + case LS_WaitLHB_ACode1: + case LS_WaitLHA_ACode1: + *acode = 1; + break; + case LS_SweepAX: + *axis = 0; + *lh = 0; + break; + case LS_SweepAY: + *axis = 1; + *lh = 0; + break; + case LS_SweepBX: + *axis = 0; + *lh = 1; + break; + case LS_SweepBY: + *axis = 1; + *lh = 1; + break; + } +} + int LighthouseState_offset(enum LighthouseState s) { int mini_jump = 20000; int big_jump = 360000; @@ -95,8 +156,9 @@ typedef struct { /** This part of the structure is general use when we know our state */ uint32_t mod_offset; enum LighthouseState state; + uint32_t last_state_transition_time; int confidence; - + uint32_t last_seen_time; /** This rest of the structure is dedicated to finding a state when we are unknown */ int encoded_acodes; @@ -299,7 +361,47 @@ static enum LighthouseState AttemptFindState(Disambiguator_data_t *d, const Ligh return LS_UNKNOWN; } -void PropagateState(Disambiguator_data_t *d, const LightcapElement *le) { d->state = LS_UNKNOWN; } +static void SetState(Disambiguator_data_t *d, const LightcapElement *le, enum LighthouseState new_state) { + + SurviveContext *ctx = d->so->ctx; + SV_INFO("State transition %d -> %d at %u(%.03f)", d->state, new_state, le->timestamp, + timestamp_diff(d->last_state_transition_time, le->timestamp) / 480000.); + + d->state = new_state; + if (d->state >= LS_END) + d->state = 1; + d->last_state_transition_time = le->timestamp; +} + +static void PropagateState(Disambiguator_data_t *d, const LightcapElement *le); +static void RunACodeCapture(int target_acode, Disambiguator_data_t *d, const LightcapElement *le) { + int acode = find_acode(le->length); + SurviveContext *ctx = d->so->ctx; + + SV_INFO("acode %d %d 0x%x", target_acode, le->length, acode); + + if (target_acode != (acode & (SKIP_BIT | AXIS_BIT))) + SetState(d, le, LS_UNKNOWN); +} + +static void PropagateState(Disambiguator_data_t *d, const LightcapElement *le) { + int acode, lh, axis, window; + LighthouseState_Parameterize(d->state, &acode, &lh, &axis, &window); + + SurviveContext *ctx = d->so->ctx; + SV_INFO("param %u %d %d %d", le->timestamp, acode, le->length, window + d->last_state_transition_time); + + if (le->timestamp < d->last_state_transition_time + window) { + if (acode != -1) { + RunACodeCapture(acode, d, le); + } else { + // RunLightDataCapture(lh, axis, d, le); + } + } else { + SetState(d, le, d->state + 1); + PropagateState(d, le); + } +} void DisambiguatorTimeBased(SurviveObject *so, const LightcapElement *le) { SurviveContext *ctx = so->ctx; @@ -315,14 +417,16 @@ void DisambiguatorTimeBased(SurviveObject *so, const LightcapElement *le) { } Disambiguator_data_t *d = so->disambiguator_data; + assert(d->last_seen_time < le->timestamp || d->last_seen_time - le->timestamp > 0x8FFFFFFF); + d->last_seen_time = le->timestamp; if (d->state == LS_UNKNOWN) { enum LighthouseState new_state = AttemptFindState(d, le); if (new_state != LS_UNKNOWN) { d->confidence = 0; d->mod_offset = (le->timestamp % LighthouseState_offset(LS_END)) - LighthouseState_offset(new_state); - d->state = new_state; - SV_INFO("Locked onto state %d at %d", new_state, d->mod_offset); + SetState(d, le, new_state); + SV_INFO("Locked onto state %d at %u", new_state, d->mod_offset); } } else { PropagateState(d, le); -- cgit v1.2.3 From 7bff842d6f01ea3a855d478734212ae0379f58e4 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Tue, 27 Mar 2018 08:53:20 -0600 Subject: Made state finder tolerant to one lh --- src/survive_tb_disambiguator.c | 79 +++++++++++++++++++++++++++--------------- 1 file changed, 51 insertions(+), 28 deletions(-) (limited to 'src') diff --git a/src/survive_tb_disambiguator.c b/src/survive_tb_disambiguator.c index e09d3da..86cc1d3 100644 --- a/src/survive_tb_disambiguator.c +++ b/src/survive_tb_disambiguator.c @@ -12,17 +12,17 @@ * The lighthouses go in the following order: * * Ticks State - * 0 ACode 0b1x0 (4) - * 20 000 ACode 0b0x0 (0) + * 0 ACode 0b1x0 (4) <--- B + * 20 000 ACode 0b0x0 (0) <--- A/c * LH A X Sweep - * 400 000 ACode 0b1x1 (5) - * 420 000 ACode 0b0x1 (1) + * 400 000 ACode 0b1x1 (5) <--- B + * 420 000 ACode 0b0x1 (1) <--- A/c * LH A Y SWEEP - * 800 000 ACode 0b0x0 (0) - * 820 000 ACode 0b1x0 (4) + * 800 000 ACode 0b0x0 (0) <--- B + * 820 000 ACode 0b1x0 (4) <--- A/c * LH B X Sweep - * 1 200 000 ACode 0b0x1 (1) - * 1 220 000 ACode 0b1x1 (5) + * 1 200 000 ACode 0b0x1 (1) <--- B + * 1 220 000 ACode 0b1x1 (5) <--- A/c * LH B Y SWEEP * 1 600 000 < REPEAT > * @@ -302,6 +302,31 @@ static enum LightcapClassification update_histories(Disambiguator_data_t *d, con return classification; } +#define ACODE(s, d, a) ((s << 2) | (d << 1) | a) +#define SWEEP 0xFF + +static enum LighthouseState CheckEncodedAcode(Disambiguator_data_t *d, uint8_t newByte) { + SurviveContext *ctx = d->so->ctx; + d->encoded_acodes &= 0xFF; + d->encoded_acodes = (d->encoded_acodes << 8) | newByte; //(acode & (SKIP_BIT | AXIS_BIT)); + SV_INFO("0x%x", d->encoded_acodes); + + switch (d->encoded_acodes) { + case (ACODE(0, 1, 0) << 8) | SWEEP: + return LS_SweepAX + 1; + case (ACODE(0, 1, 1) << 8) | SWEEP: + return LS_SweepAY + 1; + case (SWEEP << 8) | (ACODE(0, 1, 1)): + return LS_SweepBX + 1; + case (SWEEP << 8) | (ACODE(1, 1, 0)): + return LS_SweepBY + 1; + } + + return LS_UNKNOWN; +} +static enum LighthouseState EndSweep(Disambiguator_data_t *d, const LightcapElement *le) { + return CheckEncodedAcode(d, SWEEP); +} static enum LighthouseState EndSync(Disambiguator_data_t *d, const LightcapElement *le) { SurviveContext *ctx = d->so->ctx; LightcapElement lastSync = get_last_sync(d); @@ -312,24 +337,10 @@ static enum LighthouseState EndSync(Disambiguator_data_t *d, const LightcapEleme (bool)(acode & 4), acode & (SKIP_BIT | AXIS_BIT)); if (acode > 0) { - d->encoded_acodes &= 0xFFFF; - d->encoded_acodes = (d->encoded_acodes << 8) | (acode & (SKIP_BIT | AXIS_BIT)); - SV_INFO("%x", d->encoded_acodes); - switch (d->encoded_acodes) { - case (5 << 16) | (4 << 8) | 0: - return d->state = LS_SweepAX - 1; - case (0 << 16) | (5 << 8) | 1: - return d->state = LS_SweepAY - 1; - case (1 << 16) | (0 << 8) | 4: - return d->state = LS_SweepBX - 1; - case (4 << 16) | (1 << 8) | 5: - return d->state = LS_SweepBY - 1; - } - + return CheckEncodedAcode(d, (acode | DATA_BIT)); } else { d->encoded_acodes = 0; } - return LS_UNKNOWN; } @@ -340,9 +351,17 @@ static enum LighthouseState AttemptFindState(Disambiguator_data_t *d, const Ligh LightcapElement lastSync = get_last_sync(d); if (d->lastWasSync == false || overlaps(&lastSync, le) == false) { - enum LighthouseState new_state = EndSync(d, le); - if (new_state != LS_UNKNOWN) - return new_state; + if (d->lastWasSync && timestamp_diff(lastSync.timestamp, le->timestamp) > 30000) { + // Missed a sweep window; clear encoded values. + d->encoded_acodes = 0; + } + + enum LighthouseState new_state = d->lastWasSync ? EndSync(d, le) : EndSweep(d, le); + + if (new_state != LS_UNKNOWN) { + fprintf(stderr, "new state: %d\n", new_state); + } + // return new_state; d->last_sync_timestamp = le->timestamp; d->last_sync_length = le->length; @@ -355,6 +374,10 @@ static enum LighthouseState AttemptFindState(Disambiguator_data_t *d, const Ligh d->lastWasSync = true; } else { + if (d->lastWasSync) { + enum LighthouseState new_state = EndSync(d, le); + fprintf(stderr, "Sweep start\n\n"); + } d->lastWasSync = false; } @@ -417,7 +440,7 @@ void DisambiguatorTimeBased(SurviveObject *so, const LightcapElement *le) { } Disambiguator_data_t *d = so->disambiguator_data; - assert(d->last_seen_time < le->timestamp || d->last_seen_time - le->timestamp > 0x8FFFFFFF); + // assert(d->last_seen_time < le->timestamp || d->last_seen_time - le->timestamp > 0x8FFFFFFF); d->last_seen_time = le->timestamp; if (d->state == LS_UNKNOWN) { @@ -425,7 +448,7 @@ void DisambiguatorTimeBased(SurviveObject *so, const LightcapElement *le) { if (new_state != LS_UNKNOWN) { d->confidence = 0; d->mod_offset = (le->timestamp % LighthouseState_offset(LS_END)) - LighthouseState_offset(new_state); - SetState(d, le, new_state); + // SetState(d, le, new_state); SV_INFO("Locked onto state %d at %u", new_state, d->mod_offset); } } else { -- cgit v1.2.3 From d292e120dc997b4dad8d6d8a10ed86045a5f3e94 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Tue, 27 Mar 2018 16:20:41 -0600 Subject: More or less works, just doesn't output --- src/survive_tb_disambiguator.c | 298 ++++++++++++++++++++++++----------------- 1 file changed, 172 insertions(+), 126 deletions(-) (limited to 'src') diff --git a/src/survive_tb_disambiguator.c b/src/survive_tb_disambiguator.c index 86cc1d3..8bb4617 100644 --- a/src/survive_tb_disambiguator.c +++ b/src/survive_tb_disambiguator.c @@ -8,6 +8,8 @@ #define NUM_HISTORY 3 +//#define DEBUG_TB(...) SV_INFO(__VA_ARGS__) +#define DEBUG_TB(...) /** * The lighthouses go in the following order: * @@ -52,94 +54,60 @@ enum LighthouseState { LS_END }; -void LighthouseState_Parameterize(enum LighthouseState s, int *acode, int *lh, int *axis, int *window) { - *lh = *axis = *acode = -1; - switch (s) { - case LS_WaitLHB_ACode4: - case LS_WaitLHA_ACode4: - case LS_WaitLHB_ACode0: - case LS_WaitLHA_ACode0: - case LS_WaitLHB_ACode5: - case LS_WaitLHA_ACode5: - case LS_WaitLHB_ACode1: - case LS_WaitLHA_ACode1: - *window = PULSE_WINDOW; - break; - case LS_SweepAX: - case LS_SweepAY: - case LS_SweepBX: - case LS_SweepBY: - *window = CAPTURE_WINDOW; - break; - } - - switch (s) { - case LS_WaitLHB_ACode4: - case LS_WaitLHA_ACode4: - *acode = 4; - break; - case LS_WaitLHB_ACode0: - case LS_WaitLHA_ACode0: - *acode = 0; - break; - case LS_WaitLHB_ACode5: - case LS_WaitLHA_ACode5: - *acode = 5; - break; - case LS_WaitLHB_ACode1: - case LS_WaitLHA_ACode1: - *acode = 1; - break; - case LS_SweepAX: - *axis = 0; - *lh = 0; - break; - case LS_SweepAY: - *axis = 1; - *lh = 0; - break; - case LS_SweepBX: - *axis = 0; - *lh = 1; - break; - case LS_SweepBY: - *axis = 1; - *lh = 1; - break; - } -} +typedef struct { int acode, lh, axis, window, offset; } LighthouseStateParameters; + +const LighthouseStateParameters LS_Params[LS_END + 1] = { + {.acode = -1, .lh = -1, .axis = -1, .window = -1}, + + {.acode = 4, .lh = 1, .axis = 0, .window = PULSE_WINDOW, .offset = 0 * PULSE_WINDOW + 0 * CAPTURE_WINDOW}, // 0 + {.acode = 0, .lh = 0, .axis = 0, .window = PULSE_WINDOW, .offset = 1 * PULSE_WINDOW + 0 * CAPTURE_WINDOW}, // 20000 + {.acode = -1, + .lh = 0, + .axis = 0, + .window = CAPTURE_WINDOW, + .offset = 2 * PULSE_WINDOW + 0 * CAPTURE_WINDOW}, // 40000 + + {.acode = 5, .lh = 1, .axis = 1, .window = PULSE_WINDOW, .offset = 2 * PULSE_WINDOW + 1 * CAPTURE_WINDOW}, // 400000 + {.acode = 1, .lh = 0, .axis = 1, .window = PULSE_WINDOW, .offset = 3 * PULSE_WINDOW + 1 * CAPTURE_WINDOW}, // 420000 + {.acode = -1, + .lh = 0, + .axis = 1, + .window = CAPTURE_WINDOW, + .offset = 4 * PULSE_WINDOW + 1 * CAPTURE_WINDOW}, // 440000 + + {.acode = 0, .lh = 1, .axis = 0, .window = PULSE_WINDOW, .offset = 4 * PULSE_WINDOW + 2 * CAPTURE_WINDOW}, // 800000 + {.acode = 4, .lh = 0, .axis = 0, .window = PULSE_WINDOW, .offset = 5 * PULSE_WINDOW + 2 * CAPTURE_WINDOW}, // 820000 + {.acode = -1, + .lh = 1, + .axis = 0, + .window = CAPTURE_WINDOW, + .offset = 6 * PULSE_WINDOW + 2 * CAPTURE_WINDOW}, // 840000 + + {.acode = 1, + .lh = 1, + .axis = 1, + .window = PULSE_WINDOW, + .offset = 6 * PULSE_WINDOW + 3 * CAPTURE_WINDOW}, // 1200000 + {.acode = 5, + .lh = 0, + .axis = 1, + .window = PULSE_WINDOW, + .offset = 7 * PULSE_WINDOW + 3 * CAPTURE_WINDOW}, // 1220000 + {.acode = -1, + .lh = 1, + .axis = 1, + .window = CAPTURE_WINDOW, + .offset = 8 * PULSE_WINDOW + 3 * CAPTURE_WINDOW}, // 1240000 + + {.acode = -1, .lh = -1, .axis = -1, .window = -1, .offset = 8 * PULSE_WINDOW + 4 * CAPTURE_WINDOW} // 1600000 +}; -int LighthouseState_offset(enum LighthouseState s) { - int mini_jump = 20000; - int big_jump = 360000; - switch (s) { - case LS_WaitLHA_ACode4: - return 0; - case LS_WaitLHA_ACode0: - return mini_jump; - case LS_SweepAX: - return 2 * mini_jump; - case LS_WaitLHA_ACode5: - return 2 * mini_jump + big_jump; - case LS_WaitLHA_ACode1: - return 3 * mini_jump + big_jump; - case LS_SweepAY: - return 4 * mini_jump + big_jump; - case LS_WaitLHB_ACode0: - return 4 * mini_jump + 2 * big_jump; - case LS_WaitLHB_ACode4: - return 5 * mini_jump + 2 * big_jump; - case LS_SweepBX: - return 6 * mini_jump + 2 * big_jump; - case LS_WaitLHB_ACode1: - return 6 * mini_jump + 3 * big_jump; - case LS_WaitLHB_ACode5: - return 7 * mini_jump + 3 * big_jump; - case LS_SweepBY: - return 8 * mini_jump + 3 * big_jump; - case LS_END: - return 8 * mini_jump + 4 * big_jump; +enum LighthouseState LighthouseState_findByOffset(int offset) { + for (int i = 2; i < LS_END + 1; i++) { + if (LS_Params[i].offset > offset) + return i - 1; } + assert(false); return -1; } @@ -166,7 +134,7 @@ typedef struct { uint64_t last_sync_timestamp; uint64_t last_sync_length; int last_sync_count; - + int stabalize; bool lastWasSync; SensorHistory_t histories[]; @@ -186,8 +154,8 @@ static uint32_t timestamp_diff(uint32_t recent, uint32_t prior) { } static int find_acode(uint32_t pulseLen) { - const static int offset = 0; - if (pulseLen < 2200 + offset) + const static int offset = 50; + if (pulseLen < 2500 + offset) return -1; if (pulseLen < 3000 + offset) @@ -302,24 +270,46 @@ static enum LightcapClassification update_histories(Disambiguator_data_t *d, con return classification; } +#define ACODE_TIMING(acode) \ + ((3000 + ((acode)&1) * 500 + (((acode) >> 1) & 1) * 1000 + (((acode) >> 2) & 1) * 2000) - 250) #define ACODE(s, d, a) ((s << 2) | (d << 1) | a) #define SWEEP 0xFF +static uint32_t SolveForMod_Offset(Disambiguator_data_t *d, enum LighthouseState state, const LightcapElement *le) { + assert(LS_Params[state].acode >= 0); // Doesn't work for sweep data + SurviveContext *ctx = d->so->ctx; + DEBUG_TB("Solve for mod %d (%u - %u) = %u", state, le->timestamp, LS_Params[state].offset, + (le->timestamp - LS_Params[state].offset)); + + return (le->timestamp - LS_Params[state].offset); +} + +static enum LighthouseState SetState(Disambiguator_data_t *d, const LightcapElement *le, + enum LighthouseState new_state); static enum LighthouseState CheckEncodedAcode(Disambiguator_data_t *d, uint8_t newByte) { SurviveContext *ctx = d->so->ctx; d->encoded_acodes &= 0xFF; d->encoded_acodes = (d->encoded_acodes << 8) | newByte; //(acode & (SKIP_BIT | AXIS_BIT)); - SV_INFO("0x%x", d->encoded_acodes); + DEBUG_TB("0x%x", d->encoded_acodes); + LightcapElement lastSync = get_last_sync(d); switch (d->encoded_acodes) { case (ACODE(0, 1, 0) << 8) | SWEEP: - return LS_SweepAX + 1; + d->mod_offset = SolveForMod_Offset(d, LS_SweepAX - 1, &lastSync); + + return (LS_SweepAX + 1); case (ACODE(0, 1, 1) << 8) | SWEEP: - return LS_SweepAY + 1; + d->mod_offset = SolveForMod_Offset(d, LS_SweepAY - 1, &lastSync); + + return (LS_SweepAY + 1); case (SWEEP << 8) | (ACODE(0, 1, 1)): - return LS_SweepBX + 1; + d->mod_offset = SolveForMod_Offset(d, LS_WaitLHB_ACode1, &lastSync); + + return (LS_WaitLHB_ACode1 + 1); case (SWEEP << 8) | (ACODE(1, 1, 0)): - return LS_SweepBY + 1; + d->mod_offset = SolveForMod_Offset(d, LS_WaitLHA_ACode4, &lastSync); + + return (LS_WaitLHA_ACode4 + 1); } return LS_UNKNOWN; @@ -331,10 +321,10 @@ static enum LighthouseState EndSync(Disambiguator_data_t *d, const LightcapEleme SurviveContext *ctx = d->so->ctx; LightcapElement lastSync = get_last_sync(d); int acode = find_acode(lastSync.length); - SV_INFO("!!%.03f(%d)\tacode: %d 0x%x a:%d d:%d s:%d (%d)", - timestamp_diff(le->timestamp, lastSync.timestamp) / 48000., - timestamp_diff(le->timestamp, lastSync.timestamp), lastSync.length, acode, acode & 1, (bool)(acode & 2), - (bool)(acode & 4), acode & (SKIP_BIT | AXIS_BIT)); + DEBUG_TB("!!%.03f(%d)\tacode: %d 0x%x a:%d d:%d s:%d (%d)", + timestamp_diff(le->timestamp, lastSync.timestamp) / 48000., + timestamp_diff(le->timestamp, lastSync.timestamp), lastSync.length, acode, acode & 1, (bool)(acode & 2), + (bool)(acode & 4), acode & (SKIP_BIT | AXIS_BIT)); if (acode > 0) { return CheckEncodedAcode(d, (acode | DATA_BIT)); @@ -347,21 +337,29 @@ static enum LighthouseState EndSync(Disambiguator_data_t *d, const LightcapEleme static enum LighthouseState AttemptFindState(Disambiguator_data_t *d, const LightcapElement *le) { enum LightcapClassification classification = update_histories(d, le); + static uint32_t start = 0; + if (start == 0) + start = le->timestamp; + SurviveContext *ctx = d->so->ctx; + DEBUG_TB("%d(%.03f) %d Incoming %u %u", (le->timestamp - start), (le->timestamp - start) / 48000., classification, + le->timestamp, le->length); + if (classification == LCC_SYNC) { LightcapElement lastSync = get_last_sync(d); if (d->lastWasSync == false || overlaps(&lastSync, le) == false) { + if (d->lastWasSync && timestamp_diff(lastSync.timestamp, le->timestamp) > 30000) { // Missed a sweep window; clear encoded values. + SurviveContext *ctx = d->so->ctx; + // DEBUG_TB("Missed sweep window."); d->encoded_acodes = 0; } enum LighthouseState new_state = d->lastWasSync ? EndSync(d, le) : EndSweep(d, le); - if (new_state != LS_UNKNOWN) { - fprintf(stderr, "new state: %d\n", new_state); - } - // return new_state; + if (new_state != LS_UNKNOWN) + return new_state; d->last_sync_timestamp = le->timestamp; d->last_sync_length = le->length; @@ -376,7 +374,8 @@ static enum LighthouseState AttemptFindState(Disambiguator_data_t *d, const Ligh } else { if (d->lastWasSync) { enum LighthouseState new_state = EndSync(d, le); - fprintf(stderr, "Sweep start\n\n"); + if (new_state != LS_UNKNOWN) + return new_state; } d->lastWasSync = false; } @@ -384,45 +383,84 @@ static enum LighthouseState AttemptFindState(Disambiguator_data_t *d, const Ligh return LS_UNKNOWN; } -static void SetState(Disambiguator_data_t *d, const LightcapElement *le, enum LighthouseState new_state) { +static enum LighthouseState SetState(Disambiguator_data_t *d, const LightcapElement *le, + enum LighthouseState new_state) { SurviveContext *ctx = d->so->ctx; - SV_INFO("State transition %d -> %d at %u(%.03f)", d->state, new_state, le->timestamp, - timestamp_diff(d->last_state_transition_time, le->timestamp) / 480000.); + if (new_state >= LS_END) + new_state = 1; + + d->encoded_acodes = 0; + DEBUG_TB("State transition %d -> %d at %u(%.03f)", d->state, new_state, le->timestamp, + timestamp_diff(d->last_state_transition_time, le->timestamp) / 480000.); d->state = new_state; - if (d->state >= LS_END) - d->state = 1; d->last_state_transition_time = le->timestamp; + + d->last_sync_timestamp = d->last_sync_length = d->last_sync_count = 0; + + return new_state; } static void PropagateState(Disambiguator_data_t *d, const LightcapElement *le); static void RunACodeCapture(int target_acode, Disambiguator_data_t *d, const LightcapElement *le) { + if (le->length < 100) + return; + int acode = find_acode(le->length); SurviveContext *ctx = d->so->ctx; - SV_INFO("acode %d %d 0x%x", target_acode, le->length, acode); + uint32_t time_error_d0 = abs(ACODE_TIMING(target_acode) - le->length); + uint32_t time_error_d1 = abs(ACODE_TIMING(target_acode | DATA_BIT) - le->length); + uint32_t error = time_error_d0 > time_error_d1 ? time_error_d1 : time_error_d0; - if (target_acode != (acode & (SKIP_BIT | AXIS_BIT))) - SetState(d, le, LS_UNKNOWN); + DEBUG_TB("acode %d %d 0x%x (%d)", target_acode, le->length, acode, error); + if (error > 1250) { + if (d->confidence-- == 0) { + SetState(d, le, LS_UNKNOWN); + assert(false); + } + return; + } + + if (d->confidence < 100) + d->confidence++; + d->last_sync_timestamp += le->timestamp; + d->last_sync_length += le->length; + d->last_sync_count++; } static void PropagateState(Disambiguator_data_t *d, const LightcapElement *le) { - int acode, lh, axis, window; - LighthouseState_Parameterize(d->state, &acode, &lh, &axis, &window); + int le_offset = le->timestamp > d->mod_offset + ? (le->timestamp - d->mod_offset + 10000) % LS_Params[LS_END].offset + : (0xFFFFFFFF - d->mod_offset + le->timestamp + 10000) % LS_Params[LS_END].offset; + enum LighthouseState new_state = LighthouseState_findByOffset(le_offset); SurviveContext *ctx = d->so->ctx; - SV_INFO("param %u %d %d %d", le->timestamp, acode, le->length, window + d->last_state_transition_time); - - if (le->timestamp < d->last_state_transition_time + window) { - if (acode != -1) { - RunACodeCapture(acode, d, le); - } else { - // RunLightDataCapture(lh, axis, d, le); + DEBUG_TB("new %u %d %d %d %d", le->timestamp, le->length, le_offset, LS_Params[d->state].offset, + LS_Params[new_state].offset); + + if (d->state != new_state) { + if (d->last_sync_count > 0 && LS_Params[d->state].acode >= 0) { + LightcapElement lastSync = get_last_sync(d); + uint32_t mo = SolveForMod_Offset(d, d->state, &lastSync); + DEBUG_TB("New mod offset diff %d", (int)d->mod_offset - (int)mo); + d->mod_offset = mo; } + + SetState(d, le, new_state); + } + const LighthouseStateParameters *param = &LS_Params[d->state]; + + DEBUG_TB("param %u %d %d %d %d %d", le->timestamp, param->acode, le->length, le_offset, new_state, + LS_Params[d->state].offset); + + if (param->acode != -1) { + RunACodeCapture(param->acode, d, le); } else { - SetState(d, le, d->state + 1); - PropagateState(d, le); + DEBUG_TB("Logic for sweep %d", le->length); + // assert( le->length < 2200); + // RunLightDataCapture(lh, axis, d, le); } } @@ -435,21 +473,29 @@ void DisambiguatorTimeBased(SurviveObject *so, const LightcapElement *le) { } if (so->disambiguator_data == NULL) { - SV_INFO("Initializing Disambiguator Data for TB %d", so->sensor_ct); + DEBUG_TB("Initializing Disambiguator Data for TB %d", so->sensor_ct); so->disambiguator_data = Disambiguator_data_t_ctor(so); } Disambiguator_data_t *d = so->disambiguator_data; // assert(d->last_seen_time < le->timestamp || d->last_seen_time - le->timestamp > 0x8FFFFFFF); + if (d->stabalize < 500) { + d->stabalize++; + return; + } + d->last_seen_time = le->timestamp; if (d->state == LS_UNKNOWN) { enum LighthouseState new_state = AttemptFindState(d, le); if (new_state != LS_UNKNOWN) { + LightcapElement lastSync = get_last_sync(d); d->confidence = 0; - d->mod_offset = (le->timestamp % LighthouseState_offset(LS_END)) - LighthouseState_offset(new_state); - // SetState(d, le, new_state); - SV_INFO("Locked onto state %d at %u", new_state, d->mod_offset); + + int le_offset = (le->timestamp - d->mod_offset) % LS_Params[LS_END].offset; + enum LighthouseState new_state1 = LighthouseState_findByOffset(le_offset); + SetState(d, le, new_state1); + DEBUG_TB("Locked onto state %d(%d, %d) at %u", new_state, new_state1, le_offset, d->mod_offset); } } else { PropagateState(d, le); -- cgit v1.2.3 From 2e4625f76a44359fcc77b9131b18919703bba1be Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Tue, 27 Mar 2018 17:17:45 -0600 Subject: Added hooks; seems like it works but is noiser than other disambiguators? --- src/survive_tb_disambiguator.c | 110 +++++++++++++++++++++-------------------- 1 file changed, 56 insertions(+), 54 deletions(-) (limited to 'src') diff --git a/src/survive_tb_disambiguator.c b/src/survive_tb_disambiguator.c index 8bb4617..a807012 100644 --- a/src/survive_tb_disambiguator.c +++ b/src/survive_tb_disambiguator.c @@ -54,53 +54,34 @@ enum LighthouseState { LS_END }; -typedef struct { int acode, lh, axis, window, offset; } LighthouseStateParameters; +typedef struct { + int acode, lh, axis, window, offset; + bool is_sweep; +} LighthouseStateParameters; +// clang-format off const LighthouseStateParameters LS_Params[LS_END + 1] = { {.acode = -1, .lh = -1, .axis = -1, .window = -1}, - {.acode = 4, .lh = 1, .axis = 0, .window = PULSE_WINDOW, .offset = 0 * PULSE_WINDOW + 0 * CAPTURE_WINDOW}, // 0 - {.acode = 0, .lh = 0, .axis = 0, .window = PULSE_WINDOW, .offset = 1 * PULSE_WINDOW + 0 * CAPTURE_WINDOW}, // 20000 - {.acode = -1, - .lh = 0, - .axis = 0, - .window = CAPTURE_WINDOW, - .offset = 2 * PULSE_WINDOW + 0 * CAPTURE_WINDOW}, // 40000 - - {.acode = 5, .lh = 1, .axis = 1, .window = PULSE_WINDOW, .offset = 2 * PULSE_WINDOW + 1 * CAPTURE_WINDOW}, // 400000 - {.acode = 1, .lh = 0, .axis = 1, .window = PULSE_WINDOW, .offset = 3 * PULSE_WINDOW + 1 * CAPTURE_WINDOW}, // 420000 - {.acode = -1, - .lh = 0, - .axis = 1, - .window = CAPTURE_WINDOW, - .offset = 4 * PULSE_WINDOW + 1 * CAPTURE_WINDOW}, // 440000 - - {.acode = 0, .lh = 1, .axis = 0, .window = PULSE_WINDOW, .offset = 4 * PULSE_WINDOW + 2 * CAPTURE_WINDOW}, // 800000 - {.acode = 4, .lh = 0, .axis = 0, .window = PULSE_WINDOW, .offset = 5 * PULSE_WINDOW + 2 * CAPTURE_WINDOW}, // 820000 - {.acode = -1, - .lh = 1, - .axis = 0, - .window = CAPTURE_WINDOW, - .offset = 6 * PULSE_WINDOW + 2 * CAPTURE_WINDOW}, // 840000 - - {.acode = 1, - .lh = 1, - .axis = 1, - .window = PULSE_WINDOW, - .offset = 6 * PULSE_WINDOW + 3 * CAPTURE_WINDOW}, // 1200000 - {.acode = 5, - .lh = 0, - .axis = 1, - .window = PULSE_WINDOW, - .offset = 7 * PULSE_WINDOW + 3 * CAPTURE_WINDOW}, // 1220000 - {.acode = -1, - .lh = 1, - .axis = 1, - .window = CAPTURE_WINDOW, - .offset = 8 * PULSE_WINDOW + 3 * CAPTURE_WINDOW}, // 1240000 + {.acode = 4, .lh = 0, .axis = 0, .window = PULSE_WINDOW, .offset = 0 * PULSE_WINDOW + 0 * CAPTURE_WINDOW}, // 0 + {.acode = 0, .lh = 1, .axis = 0, .window = PULSE_WINDOW, .offset = 1 * PULSE_WINDOW + 0 * CAPTURE_WINDOW}, // 20000 + {.acode = 0, .lh = 1, .axis = 0, .window = CAPTURE_WINDOW, .offset = 2 * PULSE_WINDOW + 0 * CAPTURE_WINDOW,.is_sweep = 1}, // 40000 + + {.acode = 5, .lh = 0, .axis = 1, .window = PULSE_WINDOW, .offset = 2 * PULSE_WINDOW + 1 * CAPTURE_WINDOW}, // 400000 + {.acode = 1, .lh = 1, .axis = 1, .window = PULSE_WINDOW, .offset = 3 * PULSE_WINDOW + 1 * CAPTURE_WINDOW}, // 420000 + {.acode = 1, .lh = 1, .axis = 1, .window = CAPTURE_WINDOW, .offset = 4 * PULSE_WINDOW + 1 * CAPTURE_WINDOW,.is_sweep = 1}, // 440000 + + {.acode = 0, .lh = 0, .axis = 0, .window = PULSE_WINDOW, .offset = 4 * PULSE_WINDOW + 2 * CAPTURE_WINDOW}, // 800000 + {.acode = 4, .lh = 1, .axis = 0, .window = PULSE_WINDOW, .offset = 5 * PULSE_WINDOW + 2 * CAPTURE_WINDOW}, // 820000 + {.acode = 0, .lh = 0, .axis = 0, .window = CAPTURE_WINDOW, .offset = 6 * PULSE_WINDOW + 2 * CAPTURE_WINDOW,.is_sweep = 1}, // 840000 + + {.acode = 1, .lh = 0, .axis = 1, .window = PULSE_WINDOW, .offset = 6 * PULSE_WINDOW + 3 * CAPTURE_WINDOW}, // 1200000 + {.acode = 5, .lh = 1, .axis = 1, .window = PULSE_WINDOW, .offset = 7 * PULSE_WINDOW + 3 * CAPTURE_WINDOW}, // 1220000 + {.acode = 1, .lh = 0, .axis = 1, .window = CAPTURE_WINDOW, .offset = 8 * PULSE_WINDOW + 3 * CAPTURE_WINDOW,.is_sweep = 1}, // 1240000 {.acode = -1, .lh = -1, .axis = -1, .window = -1, .offset = 8 * PULSE_WINDOW + 4 * CAPTURE_WINDOW} // 1600000 }; +// clang-format on enum LighthouseState LighthouseState_findByOffset(int offset) { for (int i = 2; i < LS_END + 1; i++) { @@ -121,14 +102,16 @@ typedef struct { typedef struct { SurviveObject *so; + /** This part of the structure is general use when we know our state */ uint32_t mod_offset; enum LighthouseState state; uint32_t last_state_transition_time; int confidence; uint32_t last_seen_time; - /** This rest of the structure is dedicated to finding a state when we are unknown */ + LightcapElement sweep_data[SENSORS_PER_OBJECT]; + /** This rest of the structure is dedicated to finding a state when we are unknown */ int encoded_acodes; /* Keep running average of sync signals as they come in */ uint64_t last_sync_timestamp; @@ -276,7 +259,7 @@ static enum LightcapClassification update_histories(Disambiguator_data_t *d, con #define SWEEP 0xFF static uint32_t SolveForMod_Offset(Disambiguator_data_t *d, enum LighthouseState state, const LightcapElement *le) { - assert(LS_Params[state].acode >= 0); // Doesn't work for sweep data + assert(LS_Params[state].is_sweep == 0); // Doesn't work for sweep data SurviveContext *ctx = d->so->ctx; DEBUG_TB("Solve for mod %d (%u - %u) = %u", state, le->timestamp, LS_Params[state].offset, (le->timestamp - LS_Params[state].offset)); @@ -398,10 +381,16 @@ static enum LighthouseState SetState(Disambiguator_data_t *d, const LightcapElem d->last_state_transition_time = le->timestamp; d->last_sync_timestamp = d->last_sync_length = d->last_sync_count = 0; + memset(d->sweep_data, 0, sizeof(LightcapElement) * SENSORS_PER_OBJECT); return new_state; } +static void RunLightDataCapture(Disambiguator_data_t *d, const LightcapElement *le) { + if (le->length > d->sweep_data[le->sensor_id].length) + d->sweep_data[le->sensor_id] = *le; +} + static void PropagateState(Disambiguator_data_t *d, const LightcapElement *le); static void RunACodeCapture(int target_acode, Disambiguator_data_t *d, const LightcapElement *le) { if (le->length < 100) @@ -416,10 +405,11 @@ static void RunACodeCapture(int target_acode, Disambiguator_data_t *d, const Lig DEBUG_TB("acode %d %d 0x%x (%d)", target_acode, le->length, acode, error); if (error > 1250) { - if (d->confidence-- == 0) { + if (d->confidence < 3) { SetState(d, le, LS_UNKNOWN); assert(false); } + d->confidence -= 3; return; } @@ -441,26 +431,41 @@ static void PropagateState(Disambiguator_data_t *d, const LightcapElement *le) { LS_Params[new_state].offset); if (d->state != new_state) { - if (d->last_sync_count > 0 && LS_Params[d->state].acode >= 0) { - LightcapElement lastSync = get_last_sync(d); - uint32_t mo = SolveForMod_Offset(d, d->state, &lastSync); - DEBUG_TB("New mod offset diff %d", (int)d->mod_offset - (int)mo); - d->mod_offset = mo; + if (LS_Params[d->state].is_sweep == 0) { + if (d->last_sync_count > 0) { + LightcapElement lastSync = get_last_sync(d); + uint32_t mo = SolveForMod_Offset(d, d->state, &lastSync); + DEBUG_TB("New mod offset diff %d", (int)d->mod_offset - (int)mo); + d->mod_offset = mo; + int acode = find_acode(lastSync.length); + assert((acode | DATA_BIT) == (LS_Params[d->state].acode | DATA_BIT)); + ctx->lightproc(d->so, -LS_Params[d->state].lh - 1, acode, 0, lastSync.timestamp, lastSync.length, + LS_Params[d->state].lh); + } + } else { + for (int i = 0; i < SENSORS_PER_OBJECT; i++) { + if (d->sweep_data[i].length > 0) { + d->so->ctx->lightproc( + d->so, i, LS_Params[d->state].acode, + timestamp_diff(d->sweep_data[i].timestamp, d->mod_offset + LS_Params[d->state].offset), + d->sweep_data[i].timestamp, d->sweep_data[i].length, LS_Params[d->state].lh); + } + } } SetState(d, le, new_state); } - const LighthouseStateParameters *param = &LS_Params[d->state]; + const LighthouseStateParameters *param = &LS_Params[d->state]; DEBUG_TB("param %u %d %d %d %d %d", le->timestamp, param->acode, le->length, le_offset, new_state, LS_Params[d->state].offset); - if (param->acode != -1) { + if (param->is_sweep == 0) { RunACodeCapture(param->acode, d, le); } else { DEBUG_TB("Logic for sweep %d", le->length); // assert( le->length < 2200); - // RunLightDataCapture(lh, axis, d, le); + RunLightDataCapture(d, le); } } @@ -478,14 +483,11 @@ void DisambiguatorTimeBased(SurviveObject *so, const LightcapElement *le) { } Disambiguator_data_t *d = so->disambiguator_data; - // assert(d->last_seen_time < le->timestamp || d->last_seen_time - le->timestamp > 0x8FFFFFFF); - if (d->stabalize < 500) { d->stabalize++; return; } - d->last_seen_time = le->timestamp; if (d->state == LS_UNKNOWN) { enum LighthouseState new_state = AttemptFindState(d, le); if (new_state != LS_UNKNOWN) { -- cgit v1.2.3 From ad76877aa0f75c30c35d4d6e0f5434828e36cba7 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Wed, 28 Mar 2018 00:11:10 -0600 Subject: Finished! --- src/survive_tb_disambiguator.c | 72 ++++++++++++++++++++++-------------------- 1 file changed, 38 insertions(+), 34 deletions(-) (limited to 'src') diff --git a/src/survive_tb_disambiguator.c b/src/survive_tb_disambiguator.c index a807012..078743a 100644 --- a/src/survive_tb_disambiguator.c +++ b/src/survive_tb_disambiguator.c @@ -65,11 +65,11 @@ const LighthouseStateParameters LS_Params[LS_END + 1] = { {.acode = 4, .lh = 0, .axis = 0, .window = PULSE_WINDOW, .offset = 0 * PULSE_WINDOW + 0 * CAPTURE_WINDOW}, // 0 {.acode = 0, .lh = 1, .axis = 0, .window = PULSE_WINDOW, .offset = 1 * PULSE_WINDOW + 0 * CAPTURE_WINDOW}, // 20000 - {.acode = 0, .lh = 1, .axis = 0, .window = CAPTURE_WINDOW, .offset = 2 * PULSE_WINDOW + 0 * CAPTURE_WINDOW,.is_sweep = 1}, // 40000 + {.acode = 4, .lh = 1, .axis = 0, .window = CAPTURE_WINDOW, .offset = 2 * PULSE_WINDOW + 0 * CAPTURE_WINDOW,.is_sweep = 1}, // 40000 {.acode = 5, .lh = 0, .axis = 1, .window = PULSE_WINDOW, .offset = 2 * PULSE_WINDOW + 1 * CAPTURE_WINDOW}, // 400000 {.acode = 1, .lh = 1, .axis = 1, .window = PULSE_WINDOW, .offset = 3 * PULSE_WINDOW + 1 * CAPTURE_WINDOW}, // 420000 - {.acode = 1, .lh = 1, .axis = 1, .window = CAPTURE_WINDOW, .offset = 4 * PULSE_WINDOW + 1 * CAPTURE_WINDOW,.is_sweep = 1}, // 440000 + {.acode = 5, .lh = 1, .axis = 1, .window = CAPTURE_WINDOW, .offset = 4 * PULSE_WINDOW + 1 * CAPTURE_WINDOW,.is_sweep = 1}, // 440000 {.acode = 0, .lh = 0, .axis = 0, .window = PULSE_WINDOW, .offset = 4 * PULSE_WINDOW + 2 * CAPTURE_WINDOW}, // 800000 {.acode = 4, .lh = 1, .axis = 0, .window = PULSE_WINDOW, .offset = 5 * PULSE_WINDOW + 2 * CAPTURE_WINDOW}, // 820000 @@ -102,6 +102,7 @@ typedef struct { typedef struct { SurviveObject *so; + uint32_t time_of_last_sync[NUM_LIGHTHOUSES]; /** This part of the structure is general use when we know our state */ uint32_t mod_offset; @@ -113,6 +114,7 @@ typedef struct { /** This rest of the structure is dedicated to finding a state when we are unknown */ int encoded_acodes; + /* Keep running average of sync signals as they come in */ uint64_t last_sync_timestamp; uint64_t last_sync_length; @@ -123,13 +125,6 @@ typedef struct { } Disambiguator_data_t; -Disambiguator_data_t *Disambiguator_data_t_ctor(SurviveObject *so) { - Disambiguator_data_t *rtn = calloc(1, sizeof(Disambiguator_data_t) + sizeof(SensorHistory_t) * so->sensor_ct); - rtn->so = so; - - return rtn; -} - static uint32_t timestamp_diff(uint32_t recent, uint32_t prior) { if (recent > prior) return recent - prior; @@ -161,9 +156,6 @@ static int find_acode(uint32_t pulseLen) { return -1; } -#define LOWER_SYNC_TIME 2250 -#define UPPER_SYNC_TIME 6750 - static int circle_buffer_get(int idx, int offset) { return ((idx + offset) + NUM_HISTORY) % NUM_HISTORY; } static bool overlaps(const LightcapElement *a, const LightcapElement *b) { @@ -180,6 +172,9 @@ const int SKIP_BIT = 4; const int DATA_BIT = 2; const int AXIS_BIT = 1; +#define LOWER_SYNC_TIME 2250 +#define UPPER_SYNC_TIME 6750 + LightcapElement get_last_sync(Disambiguator_data_t *d) { if (d->last_sync_count == 0) { return (LightcapElement){0}; @@ -386,11 +381,6 @@ static enum LighthouseState SetState(Disambiguator_data_t *d, const LightcapElem return new_state; } -static void RunLightDataCapture(Disambiguator_data_t *d, const LightcapElement *le) { - if (le->length > d->sweep_data[le->sensor_id].length) - d->sweep_data[le->sensor_id] = *le; -} - static void PropagateState(Disambiguator_data_t *d, const LightcapElement *le); static void RunACodeCapture(int target_acode, Disambiguator_data_t *d, const LightcapElement *le) { if (le->length < 100) @@ -407,7 +397,8 @@ static void RunACodeCapture(int target_acode, Disambiguator_data_t *d, const Lig if (error > 1250) { if (d->confidence < 3) { SetState(d, le, LS_UNKNOWN); - assert(false); + // assert(false); + SV_INFO("WARNING: Disambiguator got lost; refinding state."); } d->confidence -= 3; return; @@ -431,24 +422,37 @@ static void PropagateState(Disambiguator_data_t *d, const LightcapElement *le) { LS_Params[new_state].offset); if (d->state != new_state) { + static uint64_t sync2syncs[LS_END] = {0}; + static uint64_t sync2syncsCnt[LS_END] = {0}; + if (LS_Params[d->state].is_sweep == 0) { if (d->last_sync_count > 0) { LightcapElement lastSync = get_last_sync(d); uint32_t mo = SolveForMod_Offset(d, d->state, &lastSync); - DEBUG_TB("New mod offset diff %d", (int)d->mod_offset - (int)mo); + DEBUG_TB("New mod offset diff %d %u", (int)d->mod_offset - (int)mo, mo); d->mod_offset = mo; - int acode = find_acode(lastSync.length); - assert((acode | DATA_BIT) == (LS_Params[d->state].acode | DATA_BIT)); + + int lengthData = ACODE_TIMING(LS_Params[d->state].acode | DATA_BIT); + int lengthNoData = ACODE_TIMING(LS_Params[d->state].acode); + + bool hasData = abs(lengthData - lastSync.length) < abs(lengthNoData - lastSync.length); + int acode = LS_Params[d->state].acode; + if (hasData) + acode |= DATA_BIT; + ctx->lightproc(d->so, -LS_Params[d->state].lh - 1, acode, 0, lastSync.timestamp, lastSync.length, LS_Params[d->state].lh); + d->time_of_last_sync[LS_Params[d->state].lh] = lastSync.timestamp; } } else { for (int i = 0; i < SENSORS_PER_OBJECT; i++) { - if (d->sweep_data[i].length > 0) { - d->so->ctx->lightproc( - d->so, i, LS_Params[d->state].acode, - timestamp_diff(d->sweep_data[i].timestamp, d->mod_offset + LS_Params[d->state].offset), - d->sweep_data[i].timestamp, d->sweep_data[i].length, LS_Params[d->state].lh); + LightcapElement le = d->sweep_data[i]; + if (le.length > 0 && d->time_of_last_sync[LS_Params[d->state].lh] > 0) { + int32_t offset_from = + timestamp_diff(le.timestamp + le.length / 2, d->time_of_last_sync[LS_Params[d->state].lh]); + assert(offset_from > 0); + d->so->ctx->lightproc(d->so, i, LS_Params[d->state].acode, offset_from, le.timestamp, le.length, + LS_Params[d->state].lh); } } } @@ -457,15 +461,12 @@ static void PropagateState(Disambiguator_data_t *d, const LightcapElement *le) { } const LighthouseStateParameters *param = &LS_Params[d->state]; - DEBUG_TB("param %u %d %d %d %d %d", le->timestamp, param->acode, le->length, le_offset, new_state, - LS_Params[d->state].offset); - if (param->is_sweep == 0) { RunACodeCapture(param->acode, d, le); } else { - DEBUG_TB("Logic for sweep %d", le->length); - // assert( le->length < 2200); - RunLightDataCapture(d, le); + if (le->length > d->sweep_data[le->sensor_id].length) { + d->sweep_data[le->sensor_id] = *le; + } } } @@ -479,11 +480,14 @@ void DisambiguatorTimeBased(SurviveObject *so, const LightcapElement *le) { if (so->disambiguator_data == NULL) { DEBUG_TB("Initializing Disambiguator Data for TB %d", so->sensor_ct); - so->disambiguator_data = Disambiguator_data_t_ctor(so); + Disambiguator_data_t *d = calloc(1, sizeof(Disambiguator_data_t) + sizeof(SensorHistory_t) * so->sensor_ct); + d->so = so; + so->disambiguator_data = d; } Disambiguator_data_t *d = so->disambiguator_data; - if (d->stabalize < 500) { + // It seems like the first few hundred lightcapelements are missing a ton of data; let it stabilize. + if (d->stabalize < 200) { d->stabalize++; return; } -- cgit v1.2.3 From bf92b059271e7be0db6f91660ad8abd1c71e8e73 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Wed, 28 Mar 2018 00:59:14 -0600 Subject: Cleanup + Comments --- src/survive_statebased_disambiguator.c | 469 ++++++++++++++++++++++++++++++ src/survive_tb_disambiguator.c | 511 --------------------------------- 2 files changed, 469 insertions(+), 511 deletions(-) create mode 100644 src/survive_statebased_disambiguator.c delete mode 100644 src/survive_tb_disambiguator.c (limited to 'src') diff --git a/src/survive_statebased_disambiguator.c b/src/survive_statebased_disambiguator.c new file mode 100644 index 0000000..4aa47ba --- /dev/null +++ b/src/survive_statebased_disambiguator.c @@ -0,0 +1,469 @@ +// +#include "survive_internal.h" +#include +#include /* for sqrt */ +#include +#include +#include + +//#define DEBUG_TB(...) SV_INFO(__VA_ARGS__) +#define DEBUG_TB(...) +/** + * The lighthouses go in the following order: + * + * Ticks State + * 0 ACode 0b1x0 (4) <--- B + * 20 000 ACode 0b0x0 (0) <--- A/c + * LH A X Sweep + * 400 000 ACode 0b1x1 (5) <--- B + * 420 000 ACode 0b0x1 (1) <--- A/c + * LH A Y SWEEP + * 800 000 ACode 0b0x0 (0) <--- B + * 820 000 ACode 0b1x0 (4) <--- A/c + * LH B X Sweep + * 1 200 000 ACode 0b0x1 (1) <--- B + * 1 220 000 ACode 0b1x1 (5) <--- A/c + * LH B Y SWEEP + * 1 600 000 < REPEAT > + * + * NOTE: Obviously you cut the data bit out for this + * + * This disambiguator works by finding where in that order it is, and tracking along with it. + * It is able to maintain this tracking for extended periods of time without further data + * by knowing the modulo of the start of the cycle and calculating appropriatly although this + * will run into issues when the timestamp rolls over or we simply drift off in accuracy. + * + * Neither case is terminal though; it will just have to find the modulo again which only takes + * a handful of pulses. + * + * The main advantage to this scheme is that its reasonably fast and is able to deal with being + * close enough to the lighthouse that the lengths are in a valid sync pulse range. + */ + +// Every pulse_window seems roughly 20k ticks long. That leaves ~360 to the capture window +#define PULSE_WINDOW 20000 +#define CAPTURE_WINDOW 360000 + +enum LighthouseState { + LS_UNKNOWN = 0, + + LS_WaitLHA_ACode4 = 1, + LS_WaitLHA_ACode0, + LS_SweepAX, + LS_WaitLHA_ACode5, + LS_WaitLHA_ACode1, + LS_SweepAY, + LS_WaitLHB_ACode0, + LS_WaitLHB_ACode4, + LS_SweepBX, + LS_WaitLHB_ACode1, + LS_WaitLHB_ACode5, + LS_SweepBY, + + LS_END +}; + +typedef struct { + int acode, lh, axis, window, offset; + bool is_sweep; +} LighthouseStateParameters; + +// clang-format off +const LighthouseStateParameters LS_Params[LS_END + 1] = { + {.acode = -1, .lh = -1, .axis = -1, .window = -1}, + + {.acode = 4, .lh = 0, .axis = 0, .window = PULSE_WINDOW, .offset = 0 * PULSE_WINDOW + 0 * CAPTURE_WINDOW}, // 0 + {.acode = 0, .lh = 1, .axis = 0, .window = PULSE_WINDOW, .offset = 1 * PULSE_WINDOW + 0 * CAPTURE_WINDOW}, // 20000 + {.acode = 4, .lh = 1, .axis = 0, .window = CAPTURE_WINDOW, .offset = 2 * PULSE_WINDOW + 0 * CAPTURE_WINDOW, .is_sweep = 1}, // 40000 + + {.acode = 5, .lh = 0, .axis = 1, .window = PULSE_WINDOW, .offset = 2 * PULSE_WINDOW + 1 * CAPTURE_WINDOW}, // 400000 + {.acode = 1, .lh = 1, .axis = 1, .window = PULSE_WINDOW, .offset = 3 * PULSE_WINDOW + 1 * CAPTURE_WINDOW}, // 420000 + {.acode = 5, .lh = 1, .axis = 1, .window = CAPTURE_WINDOW, .offset = 4 * PULSE_WINDOW + 1 * CAPTURE_WINDOW, .is_sweep = 1}, // 440000 + + {.acode = 0, .lh = 0, .axis = 0, .window = PULSE_WINDOW, .offset = 4 * PULSE_WINDOW + 2 * CAPTURE_WINDOW}, // 800000 + {.acode = 4, .lh = 1, .axis = 0, .window = PULSE_WINDOW, .offset = 5 * PULSE_WINDOW + 2 * CAPTURE_WINDOW}, // 820000 + {.acode = 0, .lh = 0, .axis = 0, .window = CAPTURE_WINDOW, .offset = 6 * PULSE_WINDOW + 2 * CAPTURE_WINDOW, .is_sweep = 1}, // 840000 + + {.acode = 1, .lh = 0, .axis = 1, .window = PULSE_WINDOW, .offset = 6 * PULSE_WINDOW + 3 * CAPTURE_WINDOW}, // 1200000 + {.acode = 5, .lh = 1, .axis = 1, .window = PULSE_WINDOW, .offset = 7 * PULSE_WINDOW + 3 * CAPTURE_WINDOW}, // 1220000 + {.acode = 1, .lh = 0, .axis = 1, .window = CAPTURE_WINDOW, .offset = 8 * PULSE_WINDOW + 3 * CAPTURE_WINDOW, .is_sweep = 1}, // 1240000 + + {.acode = -1, .lh = -1, .axis = -1, .window = -1, .offset = 8 * PULSE_WINDOW + 4 * CAPTURE_WINDOW} // 1600000 +}; +// clang-format on + +enum LighthouseState LighthouseState_findByOffset(int offset) { + for (int i = 2; i < LS_END + 1; i++) { + if (LS_Params[i].offset > offset) + return i - 1; + } + assert(false); + return -1; +} + +typedef struct { + SurviveObject *so; + /* We keep the last sync time per LH because lightproc expects numbers relative to it */ + uint32_t time_of_last_sync[NUM_LIGHTHOUSES]; + + /* Keep running average of sync signals as they come in */ + uint64_t last_sync_timestamp; + uint64_t last_sync_length; + int last_sync_count; + + /** This part of the structure is general use when we know our state */ + enum LighthouseState state; + uint32_t mod_offset; + int confidence; + + /** This rest of the structure is dedicated to finding a state when we are unknown */ + int encoded_acodes; + + int stabalize; + bool lastWasSync; + + LightcapElement sweep_data[]; +} Disambiguator_data_t; + +static uint32_t timestamp_diff(uint32_t recent, uint32_t prior) { + if (recent > prior) + return recent - prior; + return (0xFFFFFFFF - prior) + recent; +} + +static int find_acode(uint32_t pulseLen) { + const static int offset = 50; + if (pulseLen < 2500 + offset) + return -1; + + if (pulseLen < 3000 + offset) + return 0; + if (pulseLen < 3500 + offset) + return 1; + if (pulseLen < 4000 + offset) + return 2; + if (pulseLen < 4500 + offset) + return 3; + if (pulseLen < 5000 + offset) + return 4; + if (pulseLen < 5500 + offset) + return 5; + if (pulseLen < 6000 + offset) + return 6; + if (pulseLen < 6500 + offset) + return 7; + + return -1; +} + +static bool overlaps(const LightcapElement *a, const LightcapElement *b) { + int overlap = 0; + if (a->timestamp < b->timestamp && a->length + a->timestamp > b->timestamp) + overlap = a->length + a->timestamp - b->timestamp; + else if (b->timestamp < a->timestamp && b->length + b->timestamp > a->timestamp) + overlap = b->length + b->timestamp - a->timestamp; + + return overlap > a->length / 2; +} + +const int SKIP_BIT = 4; +const int DATA_BIT = 2; +const int AXIS_BIT = 1; + +#define LOWER_SYNC_TIME 2250 +#define UPPER_SYNC_TIME 6750 + +LightcapElement get_last_sync(Disambiguator_data_t *d) { + if (d->last_sync_count == 0) { + return (LightcapElement){0}; + } + + return (LightcapElement){.timestamp = (d->last_sync_timestamp + d->last_sync_count / 2) / d->last_sync_count, + .length = (d->last_sync_length + d->last_sync_count / 2) / d->last_sync_count, + .sensor_id = -d->last_sync_count}; +} + +enum LightcapClassification { LCC_SWEEP, LCC_SYNC }; +static enum LightcapClassification naive_classify(Disambiguator_data_t *d, const LightcapElement *le) { + bool clearlyNotSync = le->length < LOWER_SYNC_TIME || le->length > UPPER_SYNC_TIME; + + if (clearlyNotSync) { + return LCC_SWEEP; + } else { + return LCC_SYNC; + } +} + +#define ACODE_TIMING(acode) \ + ((3000 + ((acode)&1) * 500 + (((acode) >> 1) & 1) * 1000 + (((acode) >> 2) & 1) * 2000) - 250) +#define ACODE(s, d, a) ((s << 2) | (d << 1) | a) +#define SWEEP 0xFF + +static uint32_t SolveForMod_Offset(Disambiguator_data_t *d, enum LighthouseState state, const LightcapElement *le) { + assert(LS_Params[state].is_sweep == 0); // Doesn't work for sweep data + SurviveContext *ctx = d->so->ctx; + DEBUG_TB("Solve for mod %d (%u - %u) = %u", state, le->timestamp, LS_Params[state].offset, + (le->timestamp - LS_Params[state].offset)); + + return (le->timestamp - LS_Params[state].offset); +} + +static enum LighthouseState SetState(Disambiguator_data_t *d, const LightcapElement *le, + enum LighthouseState new_state); +static enum LighthouseState CheckEncodedAcode(Disambiguator_data_t *d, uint8_t newByte) { + + // We chain together acodes / sweep indicators to form an int we can just switch on. + SurviveContext *ctx = d->so->ctx; + d->encoded_acodes &= 0xFF; + d->encoded_acodes = (d->encoded_acodes << 8) | newByte; + + LightcapElement lastSync = get_last_sync(d); + + // These combinations are checked for specificaly to allow for the case one lighthouse is either + // missing or completely occluded. + switch (d->encoded_acodes) { + case (ACODE(0, 1, 0) << 8) | SWEEP: + d->mod_offset = SolveForMod_Offset(d, LS_SweepAX - 1, &lastSync); + + return (LS_SweepAX + 1); + case (ACODE(0, 1, 1) << 8) | SWEEP: + d->mod_offset = SolveForMod_Offset(d, LS_SweepAY - 1, &lastSync); + + return (LS_SweepAY + 1); + case (SWEEP << 8) | (ACODE(0, 1, 1)): + d->mod_offset = SolveForMod_Offset(d, LS_WaitLHB_ACode1, &lastSync); + + return (LS_WaitLHB_ACode1 + 1); + case (SWEEP << 8) | (ACODE(1, 1, 0)): + d->mod_offset = SolveForMod_Offset(d, LS_WaitLHA_ACode4, &lastSync); + + return (LS_WaitLHA_ACode4 + 1); + } + + return LS_UNKNOWN; +} +static enum LighthouseState EndSweep(Disambiguator_data_t *d, const LightcapElement *le) { + return CheckEncodedAcode(d, SWEEP); +} +static enum LighthouseState EndSync(Disambiguator_data_t *d, const LightcapElement *le) { + LightcapElement lastSync = get_last_sync(d); + int acode = find_acode(lastSync.length) > 0; + if (acode > 0) { + return CheckEncodedAcode(d, (acode | DATA_BIT)); + } else { + // If we can't resolve an acode, just reset + d->encoded_acodes = 0; + } + return LS_UNKNOWN; +} + +static enum LighthouseState AttemptFindState(Disambiguator_data_t *d, const LightcapElement *le) { + enum LightcapClassification classification = naive_classify(d, le); + + if (classification == LCC_SYNC) { + LightcapElement lastSync = get_last_sync(d); + + // Handle the case that this is a new SYNC coming in + if (d->lastWasSync == false || overlaps(&lastSync, le) == false) { + + if (d->lastWasSync && timestamp_diff(lastSync.timestamp, le->timestamp) > 30000) { + // Missed a sweep window; clear encoded values. + d->encoded_acodes = 0; + } + + // Now that the previous two states are in, check to see if they tell us where we are + enum LighthouseState new_state = d->lastWasSync ? EndSync(d, le) : EndSweep(d, le); + if (new_state != LS_UNKNOWN) + return new_state; + + // Otherwise, just reset the sync registers and do another + d->last_sync_timestamp = le->timestamp; + d->last_sync_length = le->length; + d->last_sync_count = 1; + } else { + d->last_sync_timestamp += le->timestamp; + d->last_sync_length += le->length; + d->last_sync_count++; + } + + d->lastWasSync = true; + } else { + // If this is the start of a new sweep, check to see if the end of the sync solves + // the state + if (d->lastWasSync) { + enum LighthouseState new_state = EndSync(d, le); + if (new_state != LS_UNKNOWN) + return new_state; + } + d->lastWasSync = false; + } + + return LS_UNKNOWN; +} + +static enum LighthouseState SetState(Disambiguator_data_t *d, const LightcapElement *le, + enum LighthouseState new_state) { + + SurviveContext *ctx = d->so->ctx; + if (new_state >= LS_END) + new_state = 1; + + d->encoded_acodes = 0; + d->state = new_state; + + d->last_sync_timestamp = d->last_sync_length = d->last_sync_count = 0; + memset(d->sweep_data, 0, sizeof(LightcapElement) * d->so->sensor_ct); + + return new_state; +} + +static void PropagateState(Disambiguator_data_t *d, const LightcapElement *le); +static void RunACodeCapture(int target_acode, Disambiguator_data_t *d, const LightcapElement *le) { + // Just ignore small signals; this has a measurable impact on signal quality + if (le->length < 100) + return; + + // We know what state we are in, so we verify that state as opposed to + // trying to suss out the acode. + + // Calculate what it would be with and without data + uint32_t time_error_d0 = abs(ACODE_TIMING(target_acode) - le->length); + uint32_t time_error_d1 = abs(ACODE_TIMING(target_acode | DATA_BIT) - le->length); + + // Take the least of the two erors + uint32_t error = time_error_d0 > time_error_d1 ? time_error_d1 : time_error_d0; + + // Errors do happen; either reflections or some other noise. Our scheme here is to + // keep a tally of hits and misses, and if we ever go into the negatives reset + // the state machine to find the state again. + if (error > 1250) { + // Penalize semi-harshly -- if it's ever off track it will take this many syncs + // to reset + const int penalty = 3; + if (d->confidence < penalty) { + SurviveContext *ctx = d->so->ctx; + SetState(d, le, LS_UNKNOWN); + SV_INFO("WARNING: Disambiguator got lost; refinding state."); + } + d->confidence -= penalty; + return; + } + + if (d->confidence < 100) + d->confidence++; + + // If its a real timestep, integrate it here and we can take the average later + d->last_sync_timestamp += le->timestamp; + d->last_sync_length += le->length; + d->last_sync_count++; +} + +static void ProcessStateChange(Disambiguator_data_t *d, const LightcapElement *le, enum LighthouseState new_state) { + SurviveContext *ctx = d->so->ctx; + + // Leaving a sync ... + if (LS_Params[d->state].is_sweep == 0) { + if (d->last_sync_count > 0) { + // Use the average of the captured pulse to adjust where we are modulo against. + // This lets us handle drift in any of the timing chararacteristics + LightcapElement lastSync = get_last_sync(d); + d->mod_offset = SolveForMod_Offset(d, d->state, &lastSync); + + // Figure out if it looks more like it has data or doesn't. We need this for OOX + int lengthData = ACODE_TIMING(LS_Params[d->state].acode | DATA_BIT); + int lengthNoData = ACODE_TIMING(LS_Params[d->state].acode); + bool hasData = abs(lengthData - lastSync.length) < abs(lengthNoData - lastSync.length); + int acode = LS_Params[d->state].acode; + if (hasData) { + acode |= DATA_BIT; + } + ctx->lightproc(d->so, -LS_Params[d->state].lh - 1, acode, 0, lastSync.timestamp, lastSync.length, + LS_Params[d->state].lh); + + // Store last sync time for sweep calculations + d->time_of_last_sync[LS_Params[d->state].lh] = lastSync.timestamp; + } + } else { + // Leaving a sweep ... + for (int i = 0; i < d->so->sensor_ct; i++) { + LightcapElement le = d->sweep_data[i]; + // Only care if we actually have data AND we have a time of last sync. We won't have the latter + // if we synced with the LH at cetain times. + if (le.length > 0 && d->time_of_last_sync[LS_Params[d->state].lh] > 0) { + int32_t offset_from = + timestamp_diff(le.timestamp + le.length / 2, d->time_of_last_sync[LS_Params[d->state].lh]); + + // Send the lightburst out. + assert(offset_from > 0); + d->so->ctx->lightproc(d->so, i, LS_Params[d->state].acode, offset_from, le.timestamp, le.length, + LS_Params[d->state].lh); + } + } + } + + SetState(d, le, new_state); +} + +static void PropagateState(Disambiguator_data_t *d, const LightcapElement *le) { + int le_offset = le->timestamp > d->mod_offset + ? (le->timestamp - d->mod_offset + 10000) % LS_Params[LS_END].offset + : (0xFFFFFFFF - d->mod_offset + le->timestamp + 10000) % LS_Params[LS_END].offset; + + /** Find where this new element fits into our state machine. This can skip states if its been a while since + * its been able to process, or if a LH is missing. */ + enum LighthouseState new_state = LighthouseState_findByOffset(le_offset); + + if (d->state != new_state) { + // This processes the change -- think setting buffers, and sending OOTX / lightproc calls + ProcessStateChange(d, le, new_state); + } + + const LighthouseStateParameters *param = &LS_Params[d->state]; + if (param->is_sweep == 0) { + RunACodeCapture(param->acode, d, le); + } else if (le->length > d->sweep_data[le->sensor_id].length) { + // Note we only select the highest length one per sweep. Also, we bundle everything up and send it later all at + // once. + // so that we can do this filtering. Might not be necessary? + d->sweep_data[le->sensor_id] = *le; + } +} + +void DisambiguatorStateBased(SurviveObject *so, const LightcapElement *le) { + SurviveContext *ctx = so->ctx; + + // Note, this happens if we don't have config yet -- just bail + if (so->sensor_ct == 0) { + return; + } + + if (so->disambiguator_data == NULL) { + DEBUG_TB("Initializing Disambiguator Data for TB %d", so->sensor_ct); + Disambiguator_data_t *d = calloc(1, sizeof(Disambiguator_data_t) + sizeof(LightcapElement) * so->sensor_ct); + d->so = so; + so->disambiguator_data = d; + } + + Disambiguator_data_t *d = so->disambiguator_data; + // It seems like the first few hundred lightcapelements are missing a ton of data; let it stabilize. + if (d->stabalize < 200) { + d->stabalize++; + return; + } + + if (d->state == LS_UNKNOWN) { + enum LighthouseState new_state = AttemptFindState(d, le); + if (new_state != LS_UNKNOWN) { + d->confidence = 0; + + int le_offset = (le->timestamp - d->mod_offset) % LS_Params[LS_END].offset; + enum LighthouseState new_state1 = LighthouseState_findByOffset(le_offset); + SetState(d, le, new_state1); + DEBUG_TB("Locked onto state %d(%d, %d) at %u", new_state, new_state1, le_offset, d->mod_offset); + } + } else { + PropagateState(d, le); + } +} + +REGISTER_LINKTIME(DisambiguatorStateBased); diff --git a/src/survive_tb_disambiguator.c b/src/survive_tb_disambiguator.c deleted file mode 100644 index 078743a..0000000 --- a/src/survive_tb_disambiguator.c +++ /dev/null @@ -1,511 +0,0 @@ -// -#include "survive_internal.h" -#include -#include /* for sqrt */ -#include -#include -#include - -#define NUM_HISTORY 3 - -//#define DEBUG_TB(...) SV_INFO(__VA_ARGS__) -#define DEBUG_TB(...) -/** - * The lighthouses go in the following order: - * - * Ticks State - * 0 ACode 0b1x0 (4) <--- B - * 20 000 ACode 0b0x0 (0) <--- A/c - * LH A X Sweep - * 400 000 ACode 0b1x1 (5) <--- B - * 420 000 ACode 0b0x1 (1) <--- A/c - * LH A Y SWEEP - * 800 000 ACode 0b0x0 (0) <--- B - * 820 000 ACode 0b1x0 (4) <--- A/c - * LH B X Sweep - * 1 200 000 ACode 0b0x1 (1) <--- B - * 1 220 000 ACode 0b1x1 (5) <--- A/c - * LH B Y SWEEP - * 1 600 000 < REPEAT > - * - * NOTE: Obviously you cut the data bit out for this - */ - -// Every pulse_window seems roughly 20k ticks long. That leaves ~360 to the capture window -#define PULSE_WINDOW 20000 -#define CAPTURE_WINDOW 360000 - -enum LighthouseState { - LS_UNKNOWN = 0, - - LS_WaitLHA_ACode4 = 1, - LS_WaitLHA_ACode0, - LS_SweepAX, - LS_WaitLHA_ACode5, - LS_WaitLHA_ACode1, - LS_SweepAY, - LS_WaitLHB_ACode0, - LS_WaitLHB_ACode4, - LS_SweepBX, - LS_WaitLHB_ACode1, - LS_WaitLHB_ACode5, - LS_SweepBY, - - LS_END -}; - -typedef struct { - int acode, lh, axis, window, offset; - bool is_sweep; -} LighthouseStateParameters; - -// clang-format off -const LighthouseStateParameters LS_Params[LS_END + 1] = { - {.acode = -1, .lh = -1, .axis = -1, .window = -1}, - - {.acode = 4, .lh = 0, .axis = 0, .window = PULSE_WINDOW, .offset = 0 * PULSE_WINDOW + 0 * CAPTURE_WINDOW}, // 0 - {.acode = 0, .lh = 1, .axis = 0, .window = PULSE_WINDOW, .offset = 1 * PULSE_WINDOW + 0 * CAPTURE_WINDOW}, // 20000 - {.acode = 4, .lh = 1, .axis = 0, .window = CAPTURE_WINDOW, .offset = 2 * PULSE_WINDOW + 0 * CAPTURE_WINDOW,.is_sweep = 1}, // 40000 - - {.acode = 5, .lh = 0, .axis = 1, .window = PULSE_WINDOW, .offset = 2 * PULSE_WINDOW + 1 * CAPTURE_WINDOW}, // 400000 - {.acode = 1, .lh = 1, .axis = 1, .window = PULSE_WINDOW, .offset = 3 * PULSE_WINDOW + 1 * CAPTURE_WINDOW}, // 420000 - {.acode = 5, .lh = 1, .axis = 1, .window = CAPTURE_WINDOW, .offset = 4 * PULSE_WINDOW + 1 * CAPTURE_WINDOW,.is_sweep = 1}, // 440000 - - {.acode = 0, .lh = 0, .axis = 0, .window = PULSE_WINDOW, .offset = 4 * PULSE_WINDOW + 2 * CAPTURE_WINDOW}, // 800000 - {.acode = 4, .lh = 1, .axis = 0, .window = PULSE_WINDOW, .offset = 5 * PULSE_WINDOW + 2 * CAPTURE_WINDOW}, // 820000 - {.acode = 0, .lh = 0, .axis = 0, .window = CAPTURE_WINDOW, .offset = 6 * PULSE_WINDOW + 2 * CAPTURE_WINDOW,.is_sweep = 1}, // 840000 - - {.acode = 1, .lh = 0, .axis = 1, .window = PULSE_WINDOW, .offset = 6 * PULSE_WINDOW + 3 * CAPTURE_WINDOW}, // 1200000 - {.acode = 5, .lh = 1, .axis = 1, .window = PULSE_WINDOW, .offset = 7 * PULSE_WINDOW + 3 * CAPTURE_WINDOW}, // 1220000 - {.acode = 1, .lh = 0, .axis = 1, .window = CAPTURE_WINDOW, .offset = 8 * PULSE_WINDOW + 3 * CAPTURE_WINDOW,.is_sweep = 1}, // 1240000 - - {.acode = -1, .lh = -1, .axis = -1, .window = -1, .offset = 8 * PULSE_WINDOW + 4 * CAPTURE_WINDOW} // 1600000 -}; -// clang-format on - -enum LighthouseState LighthouseState_findByOffset(int offset) { - for (int i = 2; i < LS_END + 1; i++) { - if (LS_Params[i].offset > offset) - return i - 1; - } - assert(false); - return -1; -} - -enum LightcapClassification { LCC_UNKNOWN = 0, LCC_SYNC = 1, LCC_SWEEP = 2 }; - -typedef struct { - LightcapElement history[NUM_HISTORY]; - enum LightcapClassification classifications[NUM_HISTORY]; - int idx; -} SensorHistory_t; - -typedef struct { - SurviveObject *so; - uint32_t time_of_last_sync[NUM_LIGHTHOUSES]; - - /** This part of the structure is general use when we know our state */ - uint32_t mod_offset; - enum LighthouseState state; - uint32_t last_state_transition_time; - int confidence; - uint32_t last_seen_time; - LightcapElement sweep_data[SENSORS_PER_OBJECT]; - - /** This rest of the structure is dedicated to finding a state when we are unknown */ - int encoded_acodes; - - /* Keep running average of sync signals as they come in */ - uint64_t last_sync_timestamp; - uint64_t last_sync_length; - int last_sync_count; - int stabalize; - bool lastWasSync; - SensorHistory_t histories[]; - -} Disambiguator_data_t; - -static uint32_t timestamp_diff(uint32_t recent, uint32_t prior) { - if (recent > prior) - return recent - prior; - return (0xFFFFFFFF - prior) + recent; -} - -static int find_acode(uint32_t pulseLen) { - const static int offset = 50; - if (pulseLen < 2500 + offset) - return -1; - - if (pulseLen < 3000 + offset) - return 0; - if (pulseLen < 3500 + offset) - return 1; - if (pulseLen < 4000 + offset) - return 2; - if (pulseLen < 4500 + offset) - return 3; - if (pulseLen < 5000 + offset) - return 4; - if (pulseLen < 5500 + offset) - return 5; - if (pulseLen < 6000 + offset) - return 6; - if (pulseLen < 6500 + offset) - return 7; - - return -1; -} - -static int circle_buffer_get(int idx, int offset) { return ((idx + offset) + NUM_HISTORY) % NUM_HISTORY; } - -static bool overlaps(const LightcapElement *a, const LightcapElement *b) { - int overlap = 0; - if (a->timestamp < b->timestamp && a->length + a->timestamp > b->timestamp) - overlap = a->length + a->timestamp - b->timestamp; - else if (b->timestamp < a->timestamp && b->length + b->timestamp > a->timestamp) - overlap = b->length + b->timestamp - a->timestamp; - - return overlap > a->length / 2; -} - -const int SKIP_BIT = 4; -const int DATA_BIT = 2; -const int AXIS_BIT = 1; - -#define LOWER_SYNC_TIME 2250 -#define UPPER_SYNC_TIME 6750 - -LightcapElement get_last_sync(Disambiguator_data_t *d) { - if (d->last_sync_count == 0) { - return (LightcapElement){0}; - } - - return (LightcapElement){.timestamp = (d->last_sync_timestamp + d->last_sync_count / 2) / d->last_sync_count, - .length = (d->last_sync_length + d->last_sync_count / 2) / d->last_sync_count, - .sensor_id = -d->last_sync_count}; -} - -static uint32_t next_sync_expected(Disambiguator_data_t *d) { - int acode = find_acode(get_last_sync(d).length); - if (acode & SKIP_BIT) - return get_last_sync(d).timestamp + 20000; - return get_last_sync(d).timestamp + 399840; -} - -static enum LightcapClassification classify(Disambiguator_data_t *d, SensorHistory_t *history, - const LightcapElement *le) { - bool clearlyNotSync = le->length < LOWER_SYNC_TIME || le->length > UPPER_SYNC_TIME; - - if (clearlyNotSync) { - return LCC_SWEEP; - } - - uint32_t time_diff_last_sync = timestamp_diff(le->timestamp, get_last_sync(d).timestamp); - uint32_t split_time = 399840; // 8.33ms in 48mhz - uint32_t jitter_allowance = 20000; - - // If we are ~8.33ms ahead of the last sync; we are a sync - if (get_last_sync(d).length > 0 && abs(timestamp_diff(le->timestamp, next_sync_expected(d))) < jitter_allowance) { - return LCC_SYNC; - } - - LightcapElement last_sync = get_last_sync(d); - if (get_last_sync(d).length > 0 && overlaps(&last_sync, le)) { - return LCC_SYNC; - } - - if (le->length > 2000) - return LCC_SYNC; - - if (get_last_sync(d).length > 0 && time_diff_last_sync < (split_time - jitter_allowance)) { - return LCC_SWEEP; - } - - int prevIdx = circle_buffer_get(history->idx, -1); - uint32_t time_diff = timestamp_diff(le->timestamp, history->history[prevIdx].timestamp); - - // We don't have recent data; unclear - if (time_diff > split_time) { - fprintf(stderr, "Time diff too high %d\n", time_diff); - return LCC_UNKNOWN; - } - - switch (history->classifications[prevIdx]) { - case LCC_SWEEP: - return LCC_SYNC; - } - fprintf(stderr, "last not sweep\n"); - return LCC_UNKNOWN; -} - -static enum LightcapClassification update_histories(Disambiguator_data_t *d, const LightcapElement *le) { - SensorHistory_t *history = &d->histories[le->sensor_id]; - - enum LightcapClassification classification = classify(d, history, le); - history->classifications[history->idx] = classification; - history->history[history->idx] = *le; - history->idx = (history->idx + 1) % NUM_HISTORY; - return classification; -} - -#define ACODE_TIMING(acode) \ - ((3000 + ((acode)&1) * 500 + (((acode) >> 1) & 1) * 1000 + (((acode) >> 2) & 1) * 2000) - 250) -#define ACODE(s, d, a) ((s << 2) | (d << 1) | a) -#define SWEEP 0xFF - -static uint32_t SolveForMod_Offset(Disambiguator_data_t *d, enum LighthouseState state, const LightcapElement *le) { - assert(LS_Params[state].is_sweep == 0); // Doesn't work for sweep data - SurviveContext *ctx = d->so->ctx; - DEBUG_TB("Solve for mod %d (%u - %u) = %u", state, le->timestamp, LS_Params[state].offset, - (le->timestamp - LS_Params[state].offset)); - - return (le->timestamp - LS_Params[state].offset); -} - -static enum LighthouseState SetState(Disambiguator_data_t *d, const LightcapElement *le, - enum LighthouseState new_state); -static enum LighthouseState CheckEncodedAcode(Disambiguator_data_t *d, uint8_t newByte) { - SurviveContext *ctx = d->so->ctx; - d->encoded_acodes &= 0xFF; - d->encoded_acodes = (d->encoded_acodes << 8) | newByte; //(acode & (SKIP_BIT | AXIS_BIT)); - DEBUG_TB("0x%x", d->encoded_acodes); - LightcapElement lastSync = get_last_sync(d); - - switch (d->encoded_acodes) { - case (ACODE(0, 1, 0) << 8) | SWEEP: - d->mod_offset = SolveForMod_Offset(d, LS_SweepAX - 1, &lastSync); - - return (LS_SweepAX + 1); - case (ACODE(0, 1, 1) << 8) | SWEEP: - d->mod_offset = SolveForMod_Offset(d, LS_SweepAY - 1, &lastSync); - - return (LS_SweepAY + 1); - case (SWEEP << 8) | (ACODE(0, 1, 1)): - d->mod_offset = SolveForMod_Offset(d, LS_WaitLHB_ACode1, &lastSync); - - return (LS_WaitLHB_ACode1 + 1); - case (SWEEP << 8) | (ACODE(1, 1, 0)): - d->mod_offset = SolveForMod_Offset(d, LS_WaitLHA_ACode4, &lastSync); - - return (LS_WaitLHA_ACode4 + 1); - } - - return LS_UNKNOWN; -} -static enum LighthouseState EndSweep(Disambiguator_data_t *d, const LightcapElement *le) { - return CheckEncodedAcode(d, SWEEP); -} -static enum LighthouseState EndSync(Disambiguator_data_t *d, const LightcapElement *le) { - SurviveContext *ctx = d->so->ctx; - LightcapElement lastSync = get_last_sync(d); - int acode = find_acode(lastSync.length); - DEBUG_TB("!!%.03f(%d)\tacode: %d 0x%x a:%d d:%d s:%d (%d)", - timestamp_diff(le->timestamp, lastSync.timestamp) / 48000., - timestamp_diff(le->timestamp, lastSync.timestamp), lastSync.length, acode, acode & 1, (bool)(acode & 2), - (bool)(acode & 4), acode & (SKIP_BIT | AXIS_BIT)); - - if (acode > 0) { - return CheckEncodedAcode(d, (acode | DATA_BIT)); - } else { - d->encoded_acodes = 0; - } - return LS_UNKNOWN; -} - -static enum LighthouseState AttemptFindState(Disambiguator_data_t *d, const LightcapElement *le) { - enum LightcapClassification classification = update_histories(d, le); - - static uint32_t start = 0; - if (start == 0) - start = le->timestamp; - SurviveContext *ctx = d->so->ctx; - DEBUG_TB("%d(%.03f) %d Incoming %u %u", (le->timestamp - start), (le->timestamp - start) / 48000., classification, - le->timestamp, le->length); - - if (classification == LCC_SYNC) { - LightcapElement lastSync = get_last_sync(d); - - if (d->lastWasSync == false || overlaps(&lastSync, le) == false) { - - if (d->lastWasSync && timestamp_diff(lastSync.timestamp, le->timestamp) > 30000) { - // Missed a sweep window; clear encoded values. - SurviveContext *ctx = d->so->ctx; - // DEBUG_TB("Missed sweep window."); - d->encoded_acodes = 0; - } - - enum LighthouseState new_state = d->lastWasSync ? EndSync(d, le) : EndSweep(d, le); - - if (new_state != LS_UNKNOWN) - return new_state; - - d->last_sync_timestamp = le->timestamp; - d->last_sync_length = le->length; - d->last_sync_count = 1; - } else { - d->last_sync_timestamp += le->timestamp; - d->last_sync_length += le->length; - d->last_sync_count++; - } - - d->lastWasSync = true; - } else { - if (d->lastWasSync) { - enum LighthouseState new_state = EndSync(d, le); - if (new_state != LS_UNKNOWN) - return new_state; - } - d->lastWasSync = false; - } - - return LS_UNKNOWN; -} - -static enum LighthouseState SetState(Disambiguator_data_t *d, const LightcapElement *le, - enum LighthouseState new_state) { - - SurviveContext *ctx = d->so->ctx; - if (new_state >= LS_END) - new_state = 1; - - d->encoded_acodes = 0; - DEBUG_TB("State transition %d -> %d at %u(%.03f)", d->state, new_state, le->timestamp, - timestamp_diff(d->last_state_transition_time, le->timestamp) / 480000.); - - d->state = new_state; - d->last_state_transition_time = le->timestamp; - - d->last_sync_timestamp = d->last_sync_length = d->last_sync_count = 0; - memset(d->sweep_data, 0, sizeof(LightcapElement) * SENSORS_PER_OBJECT); - - return new_state; -} - -static void PropagateState(Disambiguator_data_t *d, const LightcapElement *le); -static void RunACodeCapture(int target_acode, Disambiguator_data_t *d, const LightcapElement *le) { - if (le->length < 100) - return; - - int acode = find_acode(le->length); - SurviveContext *ctx = d->so->ctx; - - uint32_t time_error_d0 = abs(ACODE_TIMING(target_acode) - le->length); - uint32_t time_error_d1 = abs(ACODE_TIMING(target_acode | DATA_BIT) - le->length); - uint32_t error = time_error_d0 > time_error_d1 ? time_error_d1 : time_error_d0; - - DEBUG_TB("acode %d %d 0x%x (%d)", target_acode, le->length, acode, error); - if (error > 1250) { - if (d->confidence < 3) { - SetState(d, le, LS_UNKNOWN); - // assert(false); - SV_INFO("WARNING: Disambiguator got lost; refinding state."); - } - d->confidence -= 3; - return; - } - - if (d->confidence < 100) - d->confidence++; - d->last_sync_timestamp += le->timestamp; - d->last_sync_length += le->length; - d->last_sync_count++; -} - -static void PropagateState(Disambiguator_data_t *d, const LightcapElement *le) { - int le_offset = le->timestamp > d->mod_offset - ? (le->timestamp - d->mod_offset + 10000) % LS_Params[LS_END].offset - : (0xFFFFFFFF - d->mod_offset + le->timestamp + 10000) % LS_Params[LS_END].offset; - - enum LighthouseState new_state = LighthouseState_findByOffset(le_offset); - SurviveContext *ctx = d->so->ctx; - DEBUG_TB("new %u %d %d %d %d", le->timestamp, le->length, le_offset, LS_Params[d->state].offset, - LS_Params[new_state].offset); - - if (d->state != new_state) { - static uint64_t sync2syncs[LS_END] = {0}; - static uint64_t sync2syncsCnt[LS_END] = {0}; - - if (LS_Params[d->state].is_sweep == 0) { - if (d->last_sync_count > 0) { - LightcapElement lastSync = get_last_sync(d); - uint32_t mo = SolveForMod_Offset(d, d->state, &lastSync); - DEBUG_TB("New mod offset diff %d %u", (int)d->mod_offset - (int)mo, mo); - d->mod_offset = mo; - - int lengthData = ACODE_TIMING(LS_Params[d->state].acode | DATA_BIT); - int lengthNoData = ACODE_TIMING(LS_Params[d->state].acode); - - bool hasData = abs(lengthData - lastSync.length) < abs(lengthNoData - lastSync.length); - int acode = LS_Params[d->state].acode; - if (hasData) - acode |= DATA_BIT; - - ctx->lightproc(d->so, -LS_Params[d->state].lh - 1, acode, 0, lastSync.timestamp, lastSync.length, - LS_Params[d->state].lh); - d->time_of_last_sync[LS_Params[d->state].lh] = lastSync.timestamp; - } - } else { - for (int i = 0; i < SENSORS_PER_OBJECT; i++) { - LightcapElement le = d->sweep_data[i]; - if (le.length > 0 && d->time_of_last_sync[LS_Params[d->state].lh] > 0) { - int32_t offset_from = - timestamp_diff(le.timestamp + le.length / 2, d->time_of_last_sync[LS_Params[d->state].lh]); - assert(offset_from > 0); - d->so->ctx->lightproc(d->so, i, LS_Params[d->state].acode, offset_from, le.timestamp, le.length, - LS_Params[d->state].lh); - } - } - } - - SetState(d, le, new_state); - } - - const LighthouseStateParameters *param = &LS_Params[d->state]; - if (param->is_sweep == 0) { - RunACodeCapture(param->acode, d, le); - } else { - if (le->length > d->sweep_data[le->sensor_id].length) { - d->sweep_data[le->sensor_id] = *le; - } - } -} - -void DisambiguatorTimeBased(SurviveObject *so, const LightcapElement *le) { - SurviveContext *ctx = so->ctx; - - // Note, this happens if we don't have config yet -- just bail - if (so->sensor_ct == 0) { - return; - } - - if (so->disambiguator_data == NULL) { - DEBUG_TB("Initializing Disambiguator Data for TB %d", so->sensor_ct); - Disambiguator_data_t *d = calloc(1, sizeof(Disambiguator_data_t) + sizeof(SensorHistory_t) * so->sensor_ct); - d->so = so; - so->disambiguator_data = d; - } - - Disambiguator_data_t *d = so->disambiguator_data; - // It seems like the first few hundred lightcapelements are missing a ton of data; let it stabilize. - if (d->stabalize < 200) { - d->stabalize++; - return; - } - - if (d->state == LS_UNKNOWN) { - enum LighthouseState new_state = AttemptFindState(d, le); - if (new_state != LS_UNKNOWN) { - LightcapElement lastSync = get_last_sync(d); - d->confidence = 0; - - int le_offset = (le->timestamp - d->mod_offset) % LS_Params[LS_END].offset; - enum LighthouseState new_state1 = LighthouseState_findByOffset(le_offset); - SetState(d, le, new_state1); - DEBUG_TB("Locked onto state %d(%d, %d) at %u", new_state, new_state1, le_offset, d->mod_offset); - } - } else { - PropagateState(d, le); - } -} - -REGISTER_LINKTIME(DisambiguatorTimeBased); -- cgit v1.2.3