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 (limited to 'tools') 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 --- tools/disambiguate/Makefile | 2 +- tools/disambiguate/disambiguate.c | 29 +++++++++++++++++------------ 2 files changed, 18 insertions(+), 13 deletions(-) (limited to 'tools') 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 --- tools/disambiguate/disambiguate.c | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) (limited to 'tools') 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 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 --- tools/disambiguate/disambiguate.c | 15 +++++---------- 1 file changed, 5 insertions(+), 10 deletions(-) (limited to 'tools') 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