From 8688f251051fd0e12453f513b248f1c3e4f4dc7b Mon Sep 17 00:00:00 2001 From: Julian Picht Date: Mon, 26 Dec 2016 23:47:12 +0100 Subject: try to document disambiguator --- tools/disambiguate/disambiguate.c | 29 +++++++++++++++++------------ 1 file changed, 17 insertions(+), 12 deletions(-) (limited to 'tools/disambiguate/disambiguate.c') diff --git a/tools/disambiguate/disambiguate.c b/tools/disambiguate/disambiguate.c index 7e46625..4fa0758 100644 --- a/tools/disambiguate/disambiguate.c +++ b/tools/disambiguate/disambiguate.c @@ -6,51 +6,56 @@ #include #include #include -#include "../../include/disambiguator.h" +#include "disambiguator.h" int main() { - FILE * f = fopen( "raw_light_data_from_watchman.sorted.csv", "r" ); + FILE * f = fopen( "new_lightcap_data.csv", "r" ); if (f == NULL) { fprintf(stderr, "ERROR OPENING INPUT FILE\n"); return -1; } - long last = 0, lastl = 0; + uint32_t last = 0, lastl = 0; - disambiguator d; + struct disambiguator d; disambiguator_init(&d); for (;;) { char controller[10]; int sensor; int unknown; - int length; - long time; + uint32_t length; + uint32_t time; - if (fscanf(f, "%s %d %d %d %li", controller, &sensor, &unknown, &length, &time) != 5) { + if (fscanf(f, "%s %d %d %d %d", controller, &sensor, &unknown, &length, &time) != 5) { break; } if (lastl > time) { - printf("BACKWARDS: %li %li\n", lastl, time); + //printf("BACKWARDS: %li %li\n", lastl, time); } lastl = time; + if (strcmp(controller, "HMD") != 0) continue; switch (disambiguator_step(&d, time, length)) { default: case P_UNKNOWN: - //printf("UNKN %s %2d %li %d\n", controller, sensor, time - last, length); + //printf("UNKN %s %2d %d %d\n", controller, sensor, time - last, length); continue; case P_SYNC: { double l = length; char cc = round(l / 500) - 6; int ll = (length+125)/250; - printf("SYNC %s %2d %10li %5d %c%d %10li %d %d\n", controller, sensor, time, length, (cc & 0x1) ? 'k' : 'j', (cc >> 1) & 0x3, time-last, ll & 1, (ll >> 1) - 6); - last = time; + if (cc & 0x4) { + printf("SKIP %s %2d %10d %5d %c%d %10d %d %d\n", controller, sensor, time, length, (cc & 0x1) ? 'k' : 'j', (cc >> 1) & 0x3, time-last, ll & 1, (ll >> 1) - 6); + } else { + printf("SYNC %s %2d %10d %5d %c%d %10d %d %d\n", controller, sensor, time, length, (cc & 0x1) ? 'k' : 'j', (cc >> 1) & 0x3, time-last, ll & 1, (ll >> 1) - 6); + last = time; + } } continue; case P_SWEEP: - printf("SWEEP %s %2d %10li %5d\n", controller, sensor, time - last, length); + printf("SWEEP %s %2d %10d %5d\n", controller, sensor, time - last, length); continue; } } -- cgit v1.3.1 From 37e6524203b7a974bddede2f2631015d74ea31b1 Mon Sep 17 00:00:00 2001 From: Julian Picht Date: Tue, 27 Dec 2016 00:15:33 +0100 Subject: implement slave sync pulses --- src/disambiguator.c | 40 ++++++++++++++++++++++++--------------- src/disambiguator.h | 3 ++- tools/disambiguate/disambiguate.c | 11 +++++------ 3 files changed, 32 insertions(+), 22 deletions(-) (limited to 'tools/disambiguate/disambiguate.c') diff --git a/src/disambiguator.c b/src/disambiguator.c index b93639f..645b15e 100644 --- a/src/disambiguator.c +++ b/src/disambiguator.c @@ -83,55 +83,60 @@ void disambiguator_discard( struct disambiguator * d, uint32_t age ) /** * Find the index that has the best likelyhood too match up with the timestamp given - * @param time Rising edge time, where we expect to find the last sync pulse + * @param t1 Rising edge time, where we expect to find the last sync pulse, if this is a master pulse + * @param t2 Rising edge time, where we expect to find the last sync pulse, if this is a slave pulse * @param max_diff Maximum difference we are prepared to accept * @return index inside d->times, if we found something, -1 otherwise */ -inline int disambiguator_find_nearest( struct disambiguator * d, uint32_t time, int max_diff ); +inline int disambiguator_find_nearest( struct disambiguator * d, uint32_t t1, uint32_t t2, int max_diff ); -int disambiguator_find_nearest( struct disambiguator * d, uint32_t time, int max_diff ) +int disambiguator_find_nearest( struct disambiguator * d, uint32_t t1, uint32_t t2, int max_diff ) { int diff = max_diff; // max allowed diff for a match int idx = -1; for (unsigned int i = 0; i < DIS_NUM_VALUES; ++i) { if (d->times[i] == 0) continue; - int a = abs(d->times[i] - time); + int a_1 = abs(d->times[i] - t1); + int a_2 = abs(d->times[i] - t2); // printf("T %d %d %d\n", time, i, a); - if (a < diff) { + if (a_1 < diff) { idx = i; - diff = a; + diff = a_1; + } else if (a_2 < diff) { + idx = i; + diff = a_2; } } - if (idx != -1) { -// printf("R %d %d %d\n", time, idx, d->scores[idx]); - } +// if (idx != -1) { +// printf("R %d %d %d\n", idx, d->scores[idx], diff); +// } + return idx; } pulse_type disambiguator_step( struct disambiguator * d, uint32_t time, int length) { + uint32_t diff = time - d->last; + // all smaller pulses are most probably sweeps // TODO: check we are inside the time window of actual sweeps if (length < 2750) { return d->state == D_STATE_LOCKED ? P_SWEEP : P_UNKNOWN; } - // where to expect the corresponding pulse - uint32_t expected_diff = 400000; - // we expected to see a sync pulse earlier ... if (time - d->last > 401000) { d->state = D_STATE_UNLOCKED; } - + // discard all data, that is so old, we don't care about it anymore disambiguator_discard(d, time - 10000000); // find the best match for our timestamp and presumed offset - int idx = disambiguator_find_nearest(d, time - expected_diff, 100); + int idx = disambiguator_find_nearest(d, time - 400000, time - 20000, 100); // We did not find a matching pulse, so try find a place to record the current // one's time of arrival. @@ -150,10 +155,15 @@ pulse_type disambiguator_step( struct disambiguator * d, uint32_t time, int leng d->state = D_STATE_LOCKED; } + if (diff < 21000) { + return d->state == D_STATE_LOCKED ? P_SLAVE : P_UNKNOWN; + } + d->times[idx] = time; d->last = time; + return d->state == D_STATE_LOCKED ? ( - d->scores[idx] >= d->max_confidence ? P_SYNC : P_SWEEP + d->scores[idx] >= d->max_confidence ? P_MASTER : P_SWEEP ) : P_UNKNOWN; } diff --git a/src/disambiguator.h b/src/disambiguator.h index dbd4921..8258a18 100644 --- a/src/disambiguator.h +++ b/src/disambiguator.h @@ -24,8 +24,9 @@ typedef enum { */ typedef enum { P_UNKNOWN = 0, - P_SYNC = 1, + P_MASTER = 1, P_SWEEP = 2, + P_SLAVE = 3, } pulse_type; /** diff --git a/tools/disambiguate/disambiguate.c b/tools/disambiguate/disambiguate.c index 4fa0758..938a72f 100644 --- a/tools/disambiguate/disambiguate.c +++ b/tools/disambiguate/disambiguate.c @@ -41,15 +41,14 @@ int main() { case P_UNKNOWN: //printf("UNKN %s %2d %d %d\n", controller, sensor, time - last, length); continue; - case P_SYNC: + case P_MASTER: + case P_SLAVE: { - double l = length; - char cc = round(l / 500) - 6; - int ll = (length+125)/250; + char cc = (length - 2750) / 500; if (cc & 0x4) { - printf("SKIP %s %2d %10d %5d %c%d %10d %d %d\n", controller, sensor, time, length, (cc & 0x1) ? 'k' : 'j', (cc >> 1) & 0x3, time-last, ll & 1, (ll >> 1) - 6); + printf("SKIP %s %2d %10d %5d %c%d %10d\n", controller, sensor, time, length, (cc & 0x1) ? 'k' : 'j', (cc >> 1) & 0x3, time-last); } else { - printf("SYNC %s %2d %10d %5d %c%d %10d %d %d\n", controller, sensor, time, length, (cc & 0x1) ? 'k' : 'j', (cc >> 1) & 0x3, time-last, ll & 1, (ll >> 1) - 6); + printf("SYNC %s %2d %10d %5d %c%d %10d\n", controller, sensor, time, length, (cc & 0x1) ? 'k' : 'j', (cc >> 1) & 0x3, time-last); last = time; } } -- cgit v1.3.1 From 1c0f4c0ed344512153dd48941409f68bd751045a Mon Sep 17 00:00:00 2001 From: Julian Picht Date: Tue, 27 Dec 2016 00:27:15 +0100 Subject: disambiguator tool output update --- src/disambiguator.c | 1 + tools/disambiguate/disambiguate.c | 15 +++++---------- 2 files changed, 6 insertions(+), 10 deletions(-) (limited to 'tools/disambiguate/disambiguate.c') diff --git a/src/disambiguator.c b/src/disambiguator.c index 164724e..85e1645 100644 --- a/src/disambiguator.c +++ b/src/disambiguator.c @@ -161,6 +161,7 @@ pulse_type disambiguator_step( struct disambiguator * d, uint32_t time, int leng d->state = D_STATE_LOCKED; } + // if the offset is about 20000 ticks, then this is a slave pulse if (diff < 21000) { return d->state == D_STATE_LOCKED ? P_SLAVE : P_UNKNOWN; } diff --git a/tools/disambiguate/disambiguate.c b/tools/disambiguate/disambiguate.c index 938a72f..6e468dd 100644 --- a/tools/disambiguate/disambiguate.c +++ b/tools/disambiguate/disambiguate.c @@ -36,22 +36,18 @@ int main() { lastl = time; if (strcmp(controller, "HMD") != 0) continue; + char cc = (length - 2750) / 500; switch (disambiguator_step(&d, time, length)) { default: case P_UNKNOWN: //printf("UNKN %s %2d %d %d\n", controller, sensor, time - last, length); continue; case P_MASTER: + printf("MASTR %s %2d %10d %5d %c%d %10d\n", controller, sensor, time, length, (cc & 0x1) ? 'k' : 'j', (cc >> 1) & 0x3, time-last); + last = time; + continue; case P_SLAVE: - { - char cc = (length - 2750) / 500; - if (cc & 0x4) { - printf("SKIP %s %2d %10d %5d %c%d %10d\n", controller, sensor, time, length, (cc & 0x1) ? 'k' : 'j', (cc >> 1) & 0x3, time-last); - } else { - printf("SYNC %s %2d %10d %5d %c%d %10d\n", controller, sensor, time, length, (cc & 0x1) ? 'k' : 'j', (cc >> 1) & 0x3, time-last); - last = time; - } - } + printf("SLAVE %s %2d %10d %5d %c%d %10d\n", controller, sensor, time, length, (cc & 0x1) ? 'k' : 'j', (cc >> 1) & 0x3, time-last); continue; case P_SWEEP: printf("SWEEP %s %2d %10d %5d\n", controller, sensor, time - last, length); @@ -60,4 +56,3 @@ int main() { } fclose(f); } - -- cgit v1.3.1