// #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 for %s", d->so->codename); } 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 ... size_t avg_length = 0; size_t cnt = 0; 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) { avg_length += le.length; cnt++; } } if (cnt > 0) { double var = 1.5; size_t minl = (1 / var) * (avg_length + cnt / 2) / cnt; size_t maxl = var * (avg_length + cnt / 2) / cnt; 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 certain times. if (le.length > 0 && d->time_of_last_sync[LS_Params[d->state].lh] > 0 && le.length >= minl && le.length <= maxl) { 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. if (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 && le->length < 7000 /*anything above 10k seems to be bullshit?*/) { // 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);