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