From fc193f834b55a37357796446579068722354e071 Mon Sep 17 00:00:00 2001 From: Julian Picht Date: Sun, 25 Dec 2016 21:45:12 +0100 Subject: first hack of version two of disambuguate mockup --- tools/disambiguate/Makefile | 6 + tools/disambiguate/disambiguate2.c | 238 +++++++++++++++++++++++++++++++++++++ 2 files changed, 244 insertions(+) create mode 100644 tools/disambiguate/disambiguate2.c diff --git a/tools/disambiguate/Makefile b/tools/disambiguate/Makefile index 8e8661c..b06f50d 100644 --- a/tools/disambiguate/Makefile +++ b/tools/disambiguate/Makefile @@ -3,3 +3,9 @@ disambiguate: disambiguate.c ../../src/disambiguator.c test: disambiguate ./disambiguate | head -n 100 + +disambiguate2: disambiguate2.c + gcc -Wall -Wextra -Werror -g -o $@ $^ + +test2: disambiguate2 + ./disambiguate2 | head -n 100 diff --git a/tools/disambiguate/disambiguate2.c b/tools/disambiguate/disambiguate2.c new file mode 100644 index 0000000..4154314 --- /dev/null +++ b/tools/disambiguate/disambiguate2.c @@ -0,0 +1,238 @@ +#include +#include +#include + +typedef uint8_t pulse_data; + +#if 0 +struct sync_pulse_lengths { + uint32_t expected_offset; + uint16_t min; + uint16_t max; + pulse_data simple_data; + pulse_data data; +}; + +const struct sync_pulse_lengths sync_pulses[] = { + {400000, 2880, 3024, 0x0, 0x0}, // SKIP 0 DATA 0 AXIS 0 + {400000, 3380, 3524, 0x1, 0x1}, // SKIP 0 DATA 0 AXIS 1 + {400000, 3880, 4024, 0x0, 0x2}, // SKIP 0 DATA 1 AXIS 0 + {400000, 4380, 4524, 0x1, 0x3}, // SKIP 0 DATA 1 AXIS 1 + { 20000, 4880, 5024, 0x2, 0x4}, // SKIP 1 DATA 0 AXIS 0 + { 20000, 5380, 5524, 0x3, 0x5}, // SKIP 1 DATA 0 AXIS 1 + { 20000, 5880, 6024, 0x2, 0x6}, // SKIP 1 DATA 1 AXIS 0 + { 20000, 6380, 6524, 0x3, 0x7}, // SKIP 1 DATA 1 AXIS 1 +}; +#endif + +const uint32_t pulse_types[] = { + 0, 1, 0, 1, + 2, 3, 2, 3, +}; + +typedef enum { + PT_UNKNOWN = 0, + PT_SWEEP = 1, + PT_SYNC = 2 +} pulse_type; + +typedef enum { + AX_J = 0, + AX_K = 1 +} scan_axis; +typedef enum { + SK_ON = 0x4, + SK_OFF = 0 +} scan_skip; + + +#define DEBUG(...) printf(__VA_ARGS__) + +#define PULSE_BIT_AXIS 0x1 +#define PULSE_BIT_DATA 0x2 +#define PULSE_BIT_SKIP 0x4 + +#define PULSE_DATA(D) ((D >> 1)&0x1) +#define PULSE_AXIS(D) ((scan_axis)(D&0x01)) +#define PULSE_SKIP(D) ((D >> 2)&0x1) + +pulse_data get_pulse_data(uint32_t length) { + uint16_t temp = length - 2880; + +#if BETTER_SAFE_THAN_FAST + if (temp < 0 || length > 6525) { + return -1; + } +#endif + + if ((temp % 500) < 150) { + return temp / 500; + } + + return -1; +} + +struct disambiguator; +typedef pulse_type (*pulse_fn)(struct disambiguator * d, uint32_t timestamp, uint32_t length); +typedef void (*data_fn)(struct disambiguator * d, uint8_t bit); + +void null_data_fn(struct disambiguator * d __attribute__((unused)), uint8_t bit __attribute__((unused))) { + return; +} + +struct disambiguator { + pulse_fn pulse_fn; + data_fn data_fn; + uint32_t last_master_sync; + bool locked; +}; + +void disambiguator_init(struct disambiguator * d); +pulse_type disambiguator_sync_start(struct disambiguator * d, uint32_t timestamp, uint32_t length); +pulse_type disambiguator_sync_A1(struct disambiguator * d, uint32_t timestamp, uint32_t length); +pulse_type disambiguator_sync_B0(struct disambiguator * d, uint32_t timestamp, uint32_t length); +pulse_type disambiguator_sync_B1(struct disambiguator * d, uint32_t timestamp, uint32_t length); + +pulse_type disambiguator_sync_DONE(struct disambiguator * d, uint32_t timestamp, uint32_t length); + +void disambiguator_init(struct disambiguator * d) { + d->data_fn = &null_data_fn; + d->pulse_fn = &disambiguator_sync_start; + d->locked = false; +} + +pulse_type disambiguator_sync_start(struct disambiguator * d, uint32_t timestamp, uint32_t length) { + DEBUG("START %10d %6d %6d\n", timestamp, 0, length); + pulse_data dd = get_pulse_data(length); + if ((dd & (PULSE_BIT_AXIS | PULSE_BIT_SKIP)) == 0) { + d->pulse_fn = &disambiguator_sync_A1; + d->last_master_sync = timestamp; + } + return PT_UNKNOWN; +} + +pulse_type disambiguator_sync_A1(struct disambiguator * d, uint32_t timestamp, uint32_t length) { + pulse_data dd = get_pulse_data(length); + uint32_t diff = timestamp - d->last_master_sync; + DEBUG("A1 %10d %6d %6d\n", timestamp, diff, length); + if (18720 > diff) { + return PT_UNKNOWN; + } + if (diff > 21600) { + d->pulse_fn = &disambiguator_sync_start; + return PT_UNKNOWN; + } + + if ((dd & (PULSE_BIT_AXIS | PULSE_BIT_SKIP)) == PULSE_BIT_SKIP) { + d->pulse_fn = &disambiguator_sync_B0; + } + return PT_UNKNOWN; +} + +pulse_type disambiguator_sync_B0(struct disambiguator * d, uint32_t timestamp, uint32_t length) { + pulse_data dd = get_pulse_data(length); + uint32_t diff = timestamp - d->last_master_sync; + DEBUG("B0 %10d %6d %6d\n", timestamp, diff, length); + if (398832 > diff) { + return PT_UNKNOWN; + } + if (diff > 401136) { + d->pulse_fn = &disambiguator_sync_start; + return PT_UNKNOWN; + } + + if ((dd & (PULSE_BIT_AXIS | PULSE_BIT_SKIP)) == PULSE_BIT_AXIS) { + d->pulse_fn = &disambiguator_sync_B1; + d->last_master_sync = timestamp; + } + return PT_UNKNOWN; +} + +pulse_type disambiguator_sync_B1(struct disambiguator * d, uint32_t timestamp, uint32_t length) { + pulse_data dd = get_pulse_data(length); + uint32_t diff = timestamp - d->last_master_sync; + DEBUG("B1 %10d %6d %6d\n", timestamp, diff, length); + if (18720 > diff) { + return PT_UNKNOWN; + } + if (diff > 21600) { + d->pulse_fn = &disambiguator_sync_start; + return PT_UNKNOWN; + } + + if ((dd & (PULSE_BIT_AXIS | PULSE_BIT_SKIP)) == (PULSE_BIT_AXIS|PULSE_BIT_SKIP)) { + d->pulse_fn = &disambiguator_sync_DONE; + d->locked = true; + } + return PT_UNKNOWN; +} + +pulse_type disambiguator_sync_DONE(struct disambiguator * d __attribute__((unused)), uint32_t timestamp, uint32_t length) { + DEBUG("DONE %10d %6d %6d\n", timestamp, 0, length); + return PT_UNKNOWN; +} + +uint32_t fake_pulse_length(pulse_data d) { + return 3000 + (d*500); +} + + +int main() { +#ifdef TEST + struct disambiguator d; + d.data_fn = &null_data_fn; + d.pulse_fn = &disambiguator_sync_start; + d.pulse_fn(&d, 0, 3000); + d.pulse_fn(&d, 20000, fake_pulse_length(PULSE_BIT_SKIP)); + d.pulse_fn(&d, 20000, 3500); +#else + FILE * f = fopen( "raw_light_data_from_watchman.sorted.csv", "r" ); + if (f == NULL) { + fprintf(stderr, "ERROR OPENING INPUT FILE\n"); + return -1; + } + + //long last = 0, lastl = 0; + long lastl = 0; + + struct disambiguator d; + disambiguator_init(&d); + for (;;) { + char controller[10]; + int sensor; + int unknown; + int length; + long time; + + if (fscanf(f, "%s %d %d %d %li", controller, &sensor, &unknown, &length, &time) != 5) { + break; + } + printf("%s: ", controller); + if (lastl > time) { + printf("BACKWARDS: %li %li\n", lastl, time); + } + lastl = time; + + d.pulse_fn(&d, time, length); +/* switch (d.step_fn(&d, time, length)) { + default: + case P_UNKNOWN: + //printf("UNKN %s %2d %li %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; + } + continue; + case P_SWEEP: + printf("SWEEP %s %2d %10li %5d\n", controller, sensor, time - last, length); + continue; + }*/ + } + fclose(f); +#endif +} \ No newline at end of file -- cgit v1.2.3 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 --- src/disambiguator.c | 94 +++++++++++++++++++++++++++++++++------ src/disambiguator.h | 49 +++++++++++++++----- tools/disambiguate/Makefile | 2 +- tools/disambiguate/disambiguate.c | 29 +++++++----- 4 files changed, 138 insertions(+), 36 deletions(-) diff --git a/src/disambiguator.c b/src/disambiguator.c index f641834..b93639f 100644 --- a/src/disambiguator.c +++ b/src/disambiguator.c @@ -1,10 +1,54 @@ // (C) 2016 Julian Picht, MIT/x11 License. // -//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses. +// All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses. + +// +// The theory behind this disambiguator is, that if we just track all pulses and if one could be a sync pulse, we look back in time, +// if we saw as sync pulse X samples ago than it is probably a sync pulse. +// +// If the skip flag is set, X is 20000 else X is 400000 +// #include "disambiguator.h" #include #include +#include + +typedef uint8_t pulse_data; + +/** + * Translate pulse length to pulse SKIP, DATA, AXIS + * @param length Length of the pulse in (1/48000000) seconds + * @return pulse data + */ +pulse_data get_pulse_data(uint32_t length) { + uint16_t temp = length - 2880; + +#if BETTER_SAFE_THAN_FAST + if (temp < 0 || length > 6525) { + return -1; + } +#endif + + if ((temp % 500) < 150) { + return temp / 500; + } + + return -1; +} + +const uint32_t pulse_types[] = { + 0, 1, 0, 1, + 2, 3, 2, 3, +}; + +#define PULSE_BIT_AXIS 0x1 +#define PULSE_BIT_DATA 0x2 +#define PULSE_BIT_SKIP 0x4 + +#define PULSE_DATA(D) ((D >> 1)&0x1) +#define PULSE_AXIS(D) (D&0x01) +#define PULSE_SKIP(D) ((D >> 2)&0x1) void disambiguator_init( struct disambiguator * d ) { memset(&(d->times), 0x0, sizeof(d->times)); @@ -14,9 +58,14 @@ void disambiguator_init( struct disambiguator * d ) { d->max_confidence = 0; } -inline void disambiguator_discard( struct disambiguator * d, long age ); +inline void disambiguator_discard( struct disambiguator * d, uint32_t age ); -void disambiguator_discard( struct disambiguator * d, long age ) +/** + * Drop all data that is outdated + * @param d + * @param age Maximum age of data we care to keep + */ +void disambiguator_discard( struct disambiguator * d, uint32_t age ) { int confidence = 0; for (unsigned int i = 0; i < DIS_NUM_VALUES; ++i) { @@ -32,9 +81,15 @@ void disambiguator_discard( struct disambiguator * d, long age ) d->max_confidence = confidence; } -inline int disambiguator_find_nearest( struct disambiguator * d, long time, int max_diff ); +/** + * 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 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 ); -int disambiguator_find_nearest( struct disambiguator * d, long time, int max_diff ) +int disambiguator_find_nearest( struct disambiguator * d, uint32_t time, int max_diff ) { int diff = max_diff; // max allowed diff for a match int idx = -1; @@ -42,31 +97,44 @@ int disambiguator_find_nearest( struct disambiguator * d, long time, int max_dif if (d->times[i] == 0) continue; int a = abs(d->times[i] - time); + +// printf("T %d %d %d\n", time, i, a); if (a < diff) { idx = i; diff = a; } } + if (idx != -1) { +// printf("R %d %d %d\n", time, idx, d->scores[idx]); + } return idx; } -pulse_type disambiguator_step( struct disambiguator * d, long time, int length) +pulse_type disambiguator_step( struct disambiguator * d, uint32_t time, int length) { + // 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; } - //printf( "%d %d\n", time, length ); - //printf( "." ); - //time -= length/2; - disambiguator_discard(d, time - 10000000); - int idx = disambiguator_find_nearest(d, time - 400000, 100); - + // 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); + // We did not find a matching pulse, so try find a place to record the current + // one's time of arrival. if (idx == -1) { for (int i = 0; i < DIS_NUM_VALUES; ++i) { if (d->times[i] == 0) { @@ -78,7 +146,7 @@ pulse_type disambiguator_step( struct disambiguator * d, long time, int length) return d->state == D_STATE_LOCKED ? P_SWEEP : P_UNKNOWN; } else { d->scores[idx]++; - if (d->scores[idx] >= 30) { + if (d->scores[idx] >= DIS_NUM_PULSES_BEFORE_LOCK) { d->state = D_STATE_LOCKED; } diff --git a/src/disambiguator.h b/src/disambiguator.h index 0db19de..dbd4921 100644 --- a/src/disambiguator.h +++ b/src/disambiguator.h @@ -4,35 +4,64 @@ #ifndef DISAMBIGUATOR_H #define DISAMBIGUATOR_H -#define DIS_NUM_VALUES 8 +// Determines the number of samples stored in the disambiguator struct. +// Has to be higher than the maximum number of pulses expected between sync pulses. +#define DIS_NUM_VALUES 48 +#define DIS_NUM_PULSES_BEFORE_LOCK 30 +#include +/** + * internal disambiguator state + */ typedef enum { D_STATE_INVALID = 0, D_STATE_LOCKED = 1, D_STATE_UNLOCKED = -1, } dis_state; +/** + * classification result + */ typedef enum { P_UNKNOWN = 0, P_SYNC = 1, P_SWEEP = 2, } pulse_type; +/** + * internal state of the disambiguator + */ struct disambiguator { - long times[DIS_NUM_VALUES]; - int scores[DIS_NUM_VALUES]; + // the timestamps of the recorded pulses + uint32_t times[DIS_NUM_VALUES]; + // countes how many sync pulses we have seen, that match the time offset at the same offset + uint16_t scores[DIS_NUM_VALUES]; + // current state dis_state state; - long last; + // last sync pulse time + uint32_t last; + // the absolute maximum counter value int max_confidence; + // the last code type seen char code; }; -struct classified_pulse_ { - pulse_type t; - int length; -}; +/** + * Initialize a new disambiguator. calloc or memset with 0x00 will work just as well. + * + * @param d Pointer to the struct + */ void disambiguator_init( struct disambiguator * d); -pulse_type disambiguator_step( struct disambiguator * d, long time, int length); -#endif /* DISAMBIGUATOR_H */ +/** + * Feed in one pulse to have if classified. + * + * @param d Pointer to disambiguator state + * @param time Rising edge of the pulse + * @param length Length of the pulse + * @return Classification result + */ +pulse_type disambiguator_step( struct disambiguator * d, uint32_t time, int length); + +#endif /* DISAMBIGUATOR_H */ \ No newline at end of file diff --git a/tools/disambiguate/Makefile b/tools/disambiguate/Makefile index b06f50d..ef4e84c 100644 --- a/tools/disambiguate/Makefile +++ b/tools/disambiguate/Makefile @@ -1,5 +1,5 @@ disambiguate: disambiguate.c ../../src/disambiguator.c - gcc -Wall -Wextra -Werror -g -o $@ $^ -lm + gcc -I../../src -Wall -Wextra -Werror -g -o $@ $^ -lm test: disambiguate ./disambiguate | head -n 100 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.2.3 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(-) 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.2.3 From 5a1236b603b6882be2ae3af11936d135bf31b7bd Mon Sep 17 00:00:00 2001 From: Julian Picht Date: Tue, 27 Dec 2016 00:20:25 +0100 Subject: disambiguator_discard: adaptive discard --- src/disambiguator.c | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/src/disambiguator.c b/src/disambiguator.c index 645b15e..164724e 100644 --- a/src/disambiguator.c +++ b/src/disambiguator.c @@ -58,15 +58,21 @@ void disambiguator_init( struct disambiguator * d ) { d->max_confidence = 0; } -inline void disambiguator_discard( struct disambiguator * d, uint32_t age ); +inline void disambiguator_discard( struct disambiguator * d ); /** * Drop all data that is outdated * @param d * @param age Maximum age of data we care to keep */ -void disambiguator_discard( struct disambiguator * d, uint32_t age ) +void disambiguator_discard( struct disambiguator * d ) { + long age; + if (d->state == D_STATE_LOCKED) { + age = d->last - 400000; + } else { + age = 1000000; + } int confidence = 0; for (unsigned int i = 0; i < DIS_NUM_VALUES; ++i) { if (d->times[i] != 0 && d->times[i] < age) { @@ -133,7 +139,7 @@ pulse_type disambiguator_step( struct disambiguator * d, uint32_t time, int leng } // discard all data, that is so old, we don't care about it anymore - disambiguator_discard(d, time - 10000000); + disambiguator_discard(d); // find the best match for our timestamp and presumed offset int idx = disambiguator_find_nearest(d, time - 400000, time - 20000, 100); -- cgit v1.2.3 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(-) 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.2.3 From 2df2cc1a13797fd905fe89b787533635c6e6896d Mon Sep 17 00:00:00 2001 From: Julian Picht Date: Tue, 27 Dec 2016 01:05:46 +0100 Subject: my disambiguator seems to be reasonably stable now --- src/disambiguator.c | 35 +++++++++++++++++++++++++++-------- 1 file changed, 27 insertions(+), 8 deletions(-) diff --git a/src/disambiguator.c b/src/disambiguator.c index 85e1645..fd541d7 100644 --- a/src/disambiguator.c +++ b/src/disambiguator.c @@ -13,6 +13,7 @@ #include #include #include +#include typedef uint8_t pulse_data; @@ -53,6 +54,7 @@ const uint32_t pulse_types[] = { void disambiguator_init( struct disambiguator * d ) { memset(&(d->times), 0x0, sizeof(d->times)); memset(&(d->scores), 0x0, sizeof(d->scores)); + d->state = D_STATE_UNLOCKED; d->last = 0; d->max_confidence = 0; @@ -123,14 +125,22 @@ int disambiguator_find_nearest( struct disambiguator * d, uint32_t t1, uint32_t return idx; } +pulse_type disambiguator_step_return_helper( struct disambiguator * d, bool sweep_possible ) { + if (d->state == D_STATE_LOCKED && sweep_possible) { + return P_SWEEP; + } + return P_UNKNOWN; +} + pulse_type disambiguator_step( struct disambiguator * d, uint32_t time, int length) { uint32_t diff = time - d->last; + bool sweep_possible = (diff > 70000 && diff < 350000); // 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; + return disambiguator_step_return_helper(d, sweep_possible); } // we expected to see a sync pulse earlier ... @@ -142,7 +152,7 @@ pulse_type disambiguator_step( struct disambiguator * d, uint32_t time, int leng disambiguator_discard(d); // find the best match for our timestamp and presumed offset - int idx = disambiguator_find_nearest(d, time - 400000, time - 20000, 100); + int idx = disambiguator_find_nearest(d, time - 400000, time - 20000, 1000); // We did not find a matching pulse, so try find a place to record the current // one's time of arrival. @@ -154,25 +164,34 @@ pulse_type disambiguator_step( struct disambiguator * d, uint32_t time, int leng } } - return d->state == D_STATE_LOCKED ? P_SWEEP : P_UNKNOWN; + return d->state == D_STATE_LOCKED && sweep_possible ? P_SWEEP : P_UNKNOWN; } else { d->scores[idx]++; + + // we need to be reasonably sure, that we have the right pulses if (d->scores[idx] >= DIS_NUM_PULSES_BEFORE_LOCK) { 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; + if (d->state == D_STATE_LOCKED) { + return P_SLAVE; + } + + return P_UNKNOWN; } d->times[idx] = time; d->last = time; - return d->state == D_STATE_LOCKED ? ( - d->scores[idx] >= d->max_confidence ? P_MASTER : P_SWEEP - ) : P_UNKNOWN; + // TODO: why do we need to check the confidence level here? + if (d->state == D_STATE_LOCKED && d->scores[idx] >= d->max_confidence) { + return P_MASTER; + } + + return P_UNKNOWN; } - return d->state == D_STATE_LOCKED ? P_SWEEP : P_UNKNOWN; + return disambiguator_step_return_helper(d, sweep_possible); } -- cgit v1.2.3 From 5d73a53807639a3d6b989e4cd669f3ff983e62c5 Mon Sep 17 00:00:00 2001 From: Julian Picht Date: Tue, 27 Dec 2016 01:13:34 +0100 Subject: fix integration of my disambiguator --- include/survive.h | 1 + src/survive_data.c | 6 +++++- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/include/survive.h b/include/survive.h index cf2ab81..30ee310 100644 --- a/include/survive.h +++ b/include/survive.h @@ -35,6 +35,7 @@ struct SurviveObject int32_t last_photo_time; int32_t last_photo_length; #else + int32_t last_photo_time; struct disambiguator * d; #endif diff --git a/src/survive_data.c b/src/survive_data.c index 791bdf6..3e5f14e 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -43,9 +43,13 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement * int32_t offset = le->timestamp - so->d->last; switch( disambiguator_step( so->d, le->timestamp, le->length ) ) { default: + case P_SLAVE: + // this is only interesting for the OOTX data + break; case P_UNKNOWN: // not currently locked - case P_SYNC: + break; + case P_MASTER: ct->lightproc( so, le->sensor_id, -1, 0, le->timestamp, offset ); so->d->code = ((le->length+125)/250) - 12; break; -- cgit v1.2.3 From 1f141a4cd74c1c9b2d13a204da05ad697fa1c4bf Mon Sep 17 00:00:00 2001 From: Julian Picht Date: Tue, 27 Dec 2016 01:16:10 +0100 Subject: fix comment --- src/disambiguator.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/disambiguator.c b/src/disambiguator.c index fd541d7..f1d310a 100644 --- a/src/disambiguator.c +++ b/src/disambiguator.c @@ -6,7 +6,7 @@ // The theory behind this disambiguator is, that if we just track all pulses and if one could be a sync pulse, we look back in time, // if we saw as sync pulse X samples ago than it is probably a sync pulse. // -// If the skip flag is set, X is 20000 else X is 400000 +// X can be 20000 or 400000, depending if it came from the master or the slave. // #include "disambiguator.h" -- cgit v1.2.3