diff options
-rw-r--r-- | Makefile | 4 | ||||
-rw-r--r-- | src/survive_statebased_disambiguator.c (renamed from src/survive_tb_disambiguator.c) | 292 |
2 files changed, 127 insertions, 169 deletions
@@ -2,7 +2,7 @@ all : lib data_recorder test calibrate calibrate_client simple_pose_test CC?=gcc -CFLAGS:=-Iinclude/libsurvive -fPIC -g -O0 -Iredist -flto -DUSE_DOUBLE -std=gnu99 -rdynamic -llapacke -lcblas -lm #-Wall -Wno-unused-variable -Wno-switch -Wno-unused-but-set-variable -Wno-pointer-sign -Wno-parentheses +CFLAGS:=-Iinclude/libsurvive -fPIC -g -O0 -Iredist -flto -DUSE_DOUBLE -std=gnu99 -rdynamic -llapacke -lcblas -lm -fsanitize=address -fsanitize=undefined -Wall -Wno-unused-variable -Wno-switch -Wno-unused-but-set-variable -Wno-pointer-sign -Wno-parentheses CFLAGS_RELEASE:=-Iinclude/libsurvive -fPIC -msse2 -ftree-vectorize -O3 -Iredist -flto -DUSE_DOUBLE -std=gnu99 -rdynamic -llapacke -lcblas -lm @@ -39,7 +39,7 @@ REDISTS:=redist/json_helpers.o redist/linmath.o redist/jsmn.o redist/minimal_ope ifeq ($(UNAME), Darwin) REDISTS:=$(REDISTS) redist/hid-osx.c endif -LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_charlesbiguator.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_default_devices.o src/survive_vive.o src/survive_playback.o src/survive_config.o src/survive_cal.o src/survive_reproject.o src/poser.o src/epnp/epnp.c src/survive_sensor_activations.o src/survive_turveybiguator.o src/survive_disambiguator.o src/survive_tb_disambiguator.o +LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_charlesbiguator.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_default_devices.o src/survive_vive.o src/survive_playback.o src/survive_config.o src/survive_cal.o src/survive_reproject.o src/poser.o src/epnp/epnp.c src/survive_sensor_activations.o src/survive_turveybiguator.o src/survive_disambiguator.o src/survive_statebased_disambiguator.o #If you want to use HIDAPI on Linux. #CFLAGS:=$(CFLAGS) -DHIDAPI diff --git a/src/survive_tb_disambiguator.c b/src/survive_statebased_disambiguator.c index 078743a..4aa47ba 100644 --- a/src/survive_tb_disambiguator.c +++ b/src/survive_statebased_disambiguator.c @@ -6,8 +6,6 @@ #include <stdlib.h> #include <string.h> -#define NUM_HISTORY 3 - //#define DEBUG_TB(...) SV_INFO(__VA_ARGS__) #define DEBUG_TB(...) /** @@ -29,6 +27,17 @@ * 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 @@ -63,23 +72,23 @@ typedef struct { 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 = 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 = 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 = 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 = 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 + {.acode = -1, .lh = -1, .axis = -1, .window = -1, .offset = 8 * PULSE_WINDOW + 4 * CAPTURE_WINDOW} // 1600000 }; // clang-format on @@ -92,37 +101,28 @@ enum LighthouseState LighthouseState_findByOffset(int offset) { 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; + /* 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 */ - uint32_t mod_offset; enum LighthouseState state; - uint32_t last_state_transition_time; + uint32_t mod_offset; 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[]; + LightcapElement sweep_data[]; } Disambiguator_data_t; static uint32_t timestamp_diff(uint32_t recent, uint32_t prior) { @@ -156,8 +156,6 @@ static int find_acode(uint32_t pulseLen) { 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) @@ -185,67 +183,15 @@ LightcapElement get_last_sync(Disambiguator_data_t *d) { .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) { +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; - } - - 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: + } else { 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) \ @@ -265,12 +211,16 @@ static uint32_t SolveForMod_Offset(Disambiguator_data_t *d, enum LighthouseState 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; //(acode & (SKIP_BIT | AXIS_BIT)); - DEBUG_TB("0x%x", d->encoded_acodes); + 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); @@ -296,49 +246,37 @@ static enum LighthouseState EndSweep(Disambiguator_data_t *d, const LightcapElem 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)); - + 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 = 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); + 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. - SurviveContext *ctx = d->so->ctx; - // DEBUG_TB("Missed sweep window."); 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; @@ -350,6 +288,8 @@ static enum LighthouseState AttemptFindState(Disambiguator_data_t *d, const Ligh 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) @@ -369,108 +309,127 @@ static enum LighthouseState SetState(Disambiguator_data_t *d, const LightcapElem 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); + 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; - int acode = find_acode(le->length); - SurviveContext *ctx = d->so->ctx; + // 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; - DEBUG_TB("acode %d %d 0x%x (%d)", target_acode, le->length, acode, error); + // 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) { - if (d->confidence < 3) { + // 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); - // assert(false); SV_INFO("WARNING: Disambiguator got lost; refinding state."); } - d->confidence -= 3; + 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); - 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); + // 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) { - d->sweep_data[le->sensor_id] = *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 DisambiguatorTimeBased(SurviveObject *so, const LightcapElement *le) { +void DisambiguatorStateBased(SurviveObject *so, const LightcapElement *le) { SurviveContext *ctx = so->ctx; // Note, this happens if we don't have config yet -- just bail @@ -480,7 +439,7 @@ void DisambiguatorTimeBased(SurviveObject *so, const LightcapElement *le) { 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); + Disambiguator_data_t *d = calloc(1, sizeof(Disambiguator_data_t) + sizeof(LightcapElement) * so->sensor_ct); d->so = so; so->disambiguator_data = d; } @@ -495,7 +454,6 @@ void DisambiguatorTimeBased(SurviveObject *so, const LightcapElement *le) { 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; @@ -508,4 +466,4 @@ void DisambiguatorTimeBased(SurviveObject *so, const LightcapElement *le) { } } -REGISTER_LINKTIME(DisambiguatorTimeBased); +REGISTER_LINKTIME(DisambiguatorStateBased); |