From 1de8438b569e798a78d5f7dbea335539ebf68620 Mon Sep 17 00:00:00 2001 From: Julian Picht Date: Tue, 20 Dec 2016 19:50:49 +0100 Subject: pulse level debug version --- tools/disambiguate/Makefile | 2 +- tools/disambiguate/disambiguate.c | 175 ++++++++++++++++++++++++++++---------- 2 files changed, 131 insertions(+), 46 deletions(-) diff --git a/tools/disambiguate/Makefile b/tools/disambiguate/Makefile index dd5a42b..e719469 100644 --- a/tools/disambiguate/Makefile +++ b/tools/disambiguate/Makefile @@ -1,5 +1,5 @@ disambiguate: disambiguate.c - gcc -g -o $@ $^ -lm + gcc -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 30034f9..581e964 100644 --- a/tools/disambiguate/disambiguate.c +++ b/tools/disambiguate/disambiguate.c @@ -3,35 +3,117 @@ #include #include -long times[64]; -int scores[64]; - -void discard(long age) { - for (int i = 0; i < sizeof(times)/sizeof(times[0]); ++i) { - if (times[i] != 0 && times[i] < age) { - times[i] = 0; - scores[i] = 0; +#define DIS_NUM_VALUES 64 + +typedef enum { + D_SYNC_J0 = 0, + D_SYNC_K0 = 1, + D_SYNC_J1 = 2, + D_SYNC_K1 = 3, + D_SYNC_J2 = 4, + D_SYNC_K2 = 5, + D_SYNC_J3 = 6, + D_SYNC_K3 = 7, +} sync_pulse; + +typedef enum { + D_STATE_INVALID = 0, + D_STATE_LOCKED = 1, + D_STATE_UNLOCKED = -1, +} dis_state; + +typedef enum { + P_UNKNOWN = 0, + P_SYNC = 1, + P_SWEEP = 2, +} pulse_type; + +typedef struct disambiguator_ { + long times[DIS_NUM_VALUES]; + int scores[DIS_NUM_VALUES]; + dis_state state; + long last; +} disambiguator; + +typedef struct classified_pulse_ { + pulse_type t; + int length; +} classified_pulse; + +void disambiguator_init(disambiguator * d) { + memset(&(d->times), 0x0, sizeof(d->times)); + memset(&(d->scores), 0x0, sizeof(d->times)); + d->state = D_STATE_UNLOCKED; + d->last = 0; +} + +void disambiguator_discard(disambiguator * d, long age) { + for (unsigned int i = 0; i < DIS_NUM_VALUES; ++i) { + if (d->times[i] != 0 && d->times[i] < age) { + d->times[i] = 0; + d->scores[i] = 0; } } } -int findNearest(long time) { - int diff = 1000; // max allowed diff for a match +int disambiguator_find_nearest(disambiguator * d, long time, int max_diff) { + int diff = max_diff; // max allowed diff for a match int idx = -1; - for (int i = 0; i < sizeof(times)/sizeof(times[0]); ++i) { - if (times[i] == 0) continue; + for (unsigned int i = 0; i < DIS_NUM_VALUES; ++i) { + if (d->times[i] == 0) continue; - int a = abs(times[i] - time); + int a = abs(d->times[i] - time); if (a < diff) { idx = i; diff = a; } + } + + return idx; +} +pulse_type disambiguator_step(disambiguator * d, long time, int length) { + if (length < 2750) { + return d->state == D_STATE_LOCKED ? P_SWEEP : P_UNKNOWN; } - //printf("DIFF: %d %d\n", diff, idx); + disambiguator_discard(d, time - 10000000); + int idx = disambiguator_find_nearest(d, time - 400000, 100); - return idx; + if (time - d->last > 401000) { + d->state = D_STATE_UNLOCKED; + } + + if (idx == -1) { + for (int i = 0; i < DIS_NUM_VALUES; ++i) { + if (d->times[i] == 0) { + d->times[i] = time; + break; + } + } + + return d->state == D_STATE_LOCKED ? P_SWEEP : P_UNKNOWN; + } else { + // double l = length; + // char cc = round(l / 500) - 6; + // printf("MATCH: %li %d %d (0b%d%d%d)\n", time - d->times[idx], d->scores[idx], length, (cc >> 2) & 0x1, (cc >> 1) & 0x1, cc & 0x1); + d->scores[idx]++; + if (d->scores[idx] >= 30) { + d->state = D_STATE_LOCKED; + // printf("MATCH: %li %d\n", time - d->times[idx], scores[idx]); + return P_SYNC; + } + d->times[idx] = time; + d->last = time; + return d->state == D_STATE_LOCKED ? P_SYNC : P_UNKNOWN; + } + + return d->state == D_STATE_LOCKED ? P_SWEEP : P_UNKNOWN; +} + +sync_pulse length_to_pulse(int length) { + double l = length - 3000; + return round(l/500); } int main() { @@ -41,48 +123,51 @@ int main() { fprintf(stderr, "ERROR OPENING INPUT FILE\n"); return -1; } - memset(×, 0, sizeof( times )); - memset(&scores, 0, sizeof( scores )); - long offset = 0, last = 0; + long last = 0; + int num = 0; + long data[12]; + memset(&data, 0x0, sizeof(data)); + 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; } - /*if (time - last < 10000) { - offset += 0x100000000; - }*/ - time += offset; - last = time; - - if (length < 2750) continue; - - discard(time - 10000000); - int idx = findNearest(time - 800000); - if (idx == -1) { - for (int i = 0; i < sizeof(times)/sizeof(times[0]); ++i) { - if (times[i] == 0) { -// printf("ADD: %d %li\n", i, time); - times[i] = time; - break; - } - } - } else { - double l = length; - char cc = round(l / 500) - 6; - printf("MATCH: %li %d %d (0b%d%d%d)\n", time - times[idx], scores[idx], length, (cc >> 2) & 0x1, (cc >> 1) & 0x1, cc & 0x1); - scores[idx]++; - if (scores[idx] >= 30) { - printf("MATCH: %li %d\n", time - times[idx], scores[idx]); - return 0; + if (length > 2750) { + char cc = length_to_pulse(length); + printf("PULSE %d %c%d ", length, cc & 0x1 ? 'j' : 'k', (cc >> 1) & 0x3); + for (int i = 0; i < 12; ++i) { + if (data[i] != 0) + printf("%8li ", (time-data[i]) % 400000); } - times[idx] = time; + printf("\n"); + data[num++ % 12] = time; + } + + switch (disambiguator_step(&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; + printf("SYNC %s %2d %10li %c%d %10li\n", controller, sensor, time, cc & 0x1 ? 'j' : 'k', (cc >> 1) & 0x3, time-last); + last = time; + } + continue; + case P_SWEEP: + printf("SWEEP %s %2d %10li %5d\n", controller, sensor, time - last, length); + continue; } } fclose(f); -- cgit v1.2.3 From 6fbf801a0a1181861f82eab5af738a4290b21978 Mon Sep 17 00:00:00 2001 From: Julian Picht Date: Tue, 20 Dec 2016 20:01:05 +0100 Subject: looks like disambiguate is probably working pretty well(?) --- tools/disambiguate/disambiguate.c | 32 +++++++++++++++++--------------- 1 file changed, 17 insertions(+), 15 deletions(-) diff --git a/tools/disambiguate/disambiguate.c b/tools/disambiguate/disambiguate.c index 581e964..9a52c16 100644 --- a/tools/disambiguate/disambiguate.c +++ b/tools/disambiguate/disambiguate.c @@ -33,6 +33,7 @@ typedef struct disambiguator_ { int scores[DIS_NUM_VALUES]; dis_state state; long last; + int max_confidence; } disambiguator; typedef struct classified_pulse_ { @@ -45,15 +46,22 @@ void disambiguator_init(disambiguator * d) { memset(&(d->scores), 0x0, sizeof(d->times)); d->state = D_STATE_UNLOCKED; d->last = 0; + d->max_confidence = 0; } void disambiguator_discard(disambiguator * d, long age) { + int confidence = 0; for (unsigned int i = 0; i < DIS_NUM_VALUES; ++i) { if (d->times[i] != 0 && d->times[i] < age) { d->times[i] = 0; d->scores[i] = 0; + } else { + if (d->scores[i] > confidence) { + confidence = d->scores[i]; + } } } + d->max_confidence = confidence; } int disambiguator_find_nearest(disambiguator * d, long time, int max_diff) { @@ -101,11 +109,18 @@ pulse_type disambiguator_step(disambiguator * d, long time, int length) { if (d->scores[idx] >= 30) { d->state = D_STATE_LOCKED; // printf("MATCH: %li %d\n", time - d->times[idx], scores[idx]); - return P_SYNC; } +/* + for (unsigned int i = 0; i < DIS_NUM_VALUES; ++i) { + if (d->scores[i] > 0 && d->times[i] != 0) + printf("TS %2d %li %d\n", i, d->times[i], d->scores[i]); + } +*/ d->times[idx] = time; d->last = time; - return d->state == D_STATE_LOCKED ? P_SYNC : P_UNKNOWN; + return d->state == D_STATE_LOCKED ? ( + d->scores[idx] >= d->max_confidence ? P_SYNC : P_SWEEP + ) : P_UNKNOWN; } return d->state == D_STATE_LOCKED ? P_SWEEP : P_UNKNOWN; @@ -125,9 +140,6 @@ int main() { } long last = 0; - int num = 0; - long data[12]; - memset(&data, 0x0, sizeof(data)); disambiguator d; disambiguator_init(&d); @@ -141,16 +153,6 @@ int main() { if (fscanf(f, "%s %d %d %d %li", controller, &sensor, &unknown, &length, &time) != 5) { break; } - if (length > 2750) { - char cc = length_to_pulse(length); - printf("PULSE %d %c%d ", length, cc & 0x1 ? 'j' : 'k', (cc >> 1) & 0x3); - for (int i = 0; i < 12; ++i) { - if (data[i] != 0) - printf("%8li ", (time-data[i]) % 400000); - } - printf("\n"); - data[num++ % 12] = time; - } switch (disambiguator_step(&d, time, length)) { default: -- cgit v1.2.3 From a95737952d4fbb658b6de3bec312109b56caf6a6 Mon Sep 17 00:00:00 2001 From: Julian Picht Date: Tue, 20 Dec 2016 21:32:44 +0100 Subject: hacked in my disambiguator --- Makefile | 2 +- include/disambiguator.h | 35 ++++++++++ include/survive.h | 5 ++ src/disambiguator.c | 83 +++++++++++++++++++++++ src/survive_data.c | 17 +++++ tools/disambiguate/Makefile | 2 +- tools/disambiguate/disambiguate.c | 138 +++----------------------------------- 7 files changed, 150 insertions(+), 132 deletions(-) create mode 100644 include/disambiguator.h create mode 100644 src/disambiguator.c diff --git a/Makefile b/Makefile index dbd4869..90e0ec1 100644 --- a/Makefile +++ b/Makefile @@ -12,7 +12,7 @@ data_recorder : data_recorder.c lib/libsurvive.so redist/os_generic.o redist/Dra lib: mkdir lib -lib/libsurvive.so : src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o redist/jsmn.o $(DEBUGSTUFF) +lib/libsurvive.so : src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o src/disambiguator.c redist/jsmn.o $(DEBUGSTUFF) gcc -o $@ $^ $(LDFLAGS) -shared clean : diff --git a/include/disambiguator.h b/include/disambiguator.h new file mode 100644 index 0000000..58bce48 --- /dev/null +++ b/include/disambiguator.h @@ -0,0 +1,35 @@ +#ifndef DISAMBIGUATOR_H +#define DISAMBIGUATOR_H + +#define DIS_NUM_VALUES 8 + +typedef enum { + D_STATE_INVALID = 0, + D_STATE_LOCKED = 1, + D_STATE_UNLOCKED = -1, +} dis_state; + +typedef enum { + P_UNKNOWN = 0, + P_SYNC = 1, + P_SWEEP = 2, +} pulse_type; + +typedef struct disambiguator_ { + long times[DIS_NUM_VALUES]; + int scores[DIS_NUM_VALUES]; + dis_state state; + long last; + int max_confidence; + char code; +} disambiguator; + +typedef struct classified_pulse_ { + pulse_type t; + int length; +} classified_pulse; + +void disambiguator_init(disambiguator * d); +pulse_type disambiguator_step(disambiguator * d, long time, int length); + +#endif /* DISAMBIGUATOR_H */ \ No newline at end of file diff --git a/include/survive.h b/include/survive.h index f64091f..e29c82b 100644 --- a/include/survive.h +++ b/include/survive.h @@ -1,6 +1,7 @@ #ifndef _SURVIVE_H #define _SURVIVE_H +#include "disambiguator.h" #include #define SV_FLOAT double @@ -31,10 +32,14 @@ struct SurviveObject //Flood info, for calculating which laser is currently sweeping. int8_t oldcode; +#if 0 int16_t total_photos; int32_t last_photo_time; int32_t total_photo_time; int32_t total_pulsecode_time; +#else + disambiguator d; +#endif }; typedef void (*text_feedback_fnptr)( struct SurviveContext * ctx, const char * fault ); diff --git a/src/disambiguator.c b/src/disambiguator.c new file mode 100644 index 0000000..9b55fa4 --- /dev/null +++ b/src/disambiguator.c @@ -0,0 +1,83 @@ +#include "../include/disambiguator.h" +#include +#include + +void disambiguator_init(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; +} + +inline void disambiguator_discard(disambiguator * d, long age); + +void disambiguator_discard(disambiguator * d, long age) { + int confidence = 0; + for (unsigned int i = 0; i < DIS_NUM_VALUES; ++i) { + if (d->times[i] != 0 && d->times[i] < age) { + d->times[i] = 0; + d->scores[i] = 0; + } else { + if (d->scores[i] > confidence) { + confidence = d->scores[i]; + } + } + } + d->max_confidence = confidence; +} + +inline int disambiguator_find_nearest(disambiguator * d, long time, int max_diff); + +int disambiguator_find_nearest(disambiguator * d, long time, 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); + if (a < diff) { + idx = i; + diff = a; + } + } + + return idx; +} + +pulse_type disambiguator_step(disambiguator * d, long time, int length) { + if (length < 2750) { + return d->state == D_STATE_LOCKED ? P_SWEEP : P_UNKNOWN; + } + + disambiguator_discard(d, time - 10000000); + int idx = disambiguator_find_nearest(d, time - 400000, 100); + + if (time - d->last > 401000) { + d->state = D_STATE_UNLOCKED; + } + + if (idx == -1) { + for (int i = 0; i < DIS_NUM_VALUES; ++i) { + if (d->times[i] == 0) { + d->times[i] = time; + break; + } + } + + return d->state == D_STATE_LOCKED ? P_SWEEP : P_UNKNOWN; + } else { + d->scores[idx]++; + if (d->scores[idx] >= 30) { + d->state = D_STATE_LOCKED; + } + + d->times[idx] = time; + d->last = time; + return d->state == D_STATE_LOCKED ? ( + d->scores[idx] >= d->max_confidence ? P_SYNC : P_SWEEP + ) : P_UNKNOWN; + } + + return d->state == D_STATE_LOCKED ? P_SWEEP : P_UNKNOWN; +} diff --git a/src/survive_data.c b/src/survive_data.c index 660c3e1..fc86d9f 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -11,6 +11,7 @@ //All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses. #include "survive_internal.h" +#include "disambiguator.h" #include #include @@ -41,6 +42,21 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement * if( le->type != 0xfe || le->length < 50 ) return; //le->timestamp += (le->length/2); + int32_t offset = le->timestamp - so->d.last; + switch(disambiguator_step(&(so->d), le->timestamp, le->length)) { + default: + case P_UNKNOWN: + // not currently locked + case P_SYNC: + ct->lightproc( so, le->sensor_id, -1, 0, le->timestamp, offset ); + so->d.code = ((le->length+125)/250) - 12; + break; + case P_SWEEP: + if (so->d.gcode & 1) return; + ct->lightproc( so, le->sensor_id, so->d.code >> 1, offset, le->timestamp, le->length ); + break; + } +#if 0 if( le->length > 2100 ) //Pulse longer indicates a sync pulse. { int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_photo_time; @@ -85,6 +101,7 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement * { //Runt pulse. } +#endif } diff --git a/tools/disambiguate/Makefile b/tools/disambiguate/Makefile index e719469..8e8661c 100644 --- a/tools/disambiguate/Makefile +++ b/tools/disambiguate/Makefile @@ -1,4 +1,4 @@ -disambiguate: disambiguate.c +disambiguate: disambiguate.c ../../src/disambiguator.c gcc -Wall -Wextra -Werror -g -o $@ $^ -lm test: disambiguate diff --git a/tools/disambiguate/disambiguate.c b/tools/disambiguate/disambiguate.c index 9a52c16..dcaf9d8 100644 --- a/tools/disambiguate/disambiguate.c +++ b/tools/disambiguate/disambiguate.c @@ -2,134 +2,7 @@ #include #include #include - -#define DIS_NUM_VALUES 64 - -typedef enum { - D_SYNC_J0 = 0, - D_SYNC_K0 = 1, - D_SYNC_J1 = 2, - D_SYNC_K1 = 3, - D_SYNC_J2 = 4, - D_SYNC_K2 = 5, - D_SYNC_J3 = 6, - D_SYNC_K3 = 7, -} sync_pulse; - -typedef enum { - D_STATE_INVALID = 0, - D_STATE_LOCKED = 1, - D_STATE_UNLOCKED = -1, -} dis_state; - -typedef enum { - P_UNKNOWN = 0, - P_SYNC = 1, - P_SWEEP = 2, -} pulse_type; - -typedef struct disambiguator_ { - long times[DIS_NUM_VALUES]; - int scores[DIS_NUM_VALUES]; - dis_state state; - long last; - int max_confidence; -} disambiguator; - -typedef struct classified_pulse_ { - pulse_type t; - int length; -} classified_pulse; - -void disambiguator_init(disambiguator * d) { - memset(&(d->times), 0x0, sizeof(d->times)); - memset(&(d->scores), 0x0, sizeof(d->times)); - d->state = D_STATE_UNLOCKED; - d->last = 0; - d->max_confidence = 0; -} - -void disambiguator_discard(disambiguator * d, long age) { - int confidence = 0; - for (unsigned int i = 0; i < DIS_NUM_VALUES; ++i) { - if (d->times[i] != 0 && d->times[i] < age) { - d->times[i] = 0; - d->scores[i] = 0; - } else { - if (d->scores[i] > confidence) { - confidence = d->scores[i]; - } - } - } - d->max_confidence = confidence; -} - -int disambiguator_find_nearest(disambiguator * d, long time, 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); - if (a < diff) { - idx = i; - diff = a; - } - } - - return idx; -} - -pulse_type disambiguator_step(disambiguator * d, long time, int length) { - if (length < 2750) { - return d->state == D_STATE_LOCKED ? P_SWEEP : P_UNKNOWN; - } - - disambiguator_discard(d, time - 10000000); - int idx = disambiguator_find_nearest(d, time - 400000, 100); - - if (time - d->last > 401000) { - d->state = D_STATE_UNLOCKED; - } - - if (idx == -1) { - for (int i = 0; i < DIS_NUM_VALUES; ++i) { - if (d->times[i] == 0) { - d->times[i] = time; - break; - } - } - - return d->state == D_STATE_LOCKED ? P_SWEEP : P_UNKNOWN; - } else { - // double l = length; - // char cc = round(l / 500) - 6; - // printf("MATCH: %li %d %d (0b%d%d%d)\n", time - d->times[idx], d->scores[idx], length, (cc >> 2) & 0x1, (cc >> 1) & 0x1, cc & 0x1); - d->scores[idx]++; - if (d->scores[idx] >= 30) { - d->state = D_STATE_LOCKED; - // printf("MATCH: %li %d\n", time - d->times[idx], scores[idx]); - } -/* - for (unsigned int i = 0; i < DIS_NUM_VALUES; ++i) { - if (d->scores[i] > 0 && d->times[i] != 0) - printf("TS %2d %li %d\n", i, d->times[i], d->scores[i]); - } -*/ - d->times[idx] = time; - d->last = time; - return d->state == D_STATE_LOCKED ? ( - d->scores[idx] >= d->max_confidence ? P_SYNC : P_SWEEP - ) : P_UNKNOWN; - } - - return d->state == D_STATE_LOCKED ? P_SWEEP : P_UNKNOWN; -} - -sync_pulse length_to_pulse(int length) { - double l = length - 3000; - return round(l/500); -} +#include "../../include/disambiguator.h" int main() { @@ -139,7 +12,7 @@ int main() { return -1; } - long last = 0; + long last = 0, lastl = 0; disambiguator d; disambiguator_init(&d); @@ -153,6 +26,10 @@ int main() { if (fscanf(f, "%s %d %d %d %li", controller, &sensor, &unknown, &length, &time) != 5) { break; } + if (lastl > time) { + printf("BACKWARDS: %li %li\n", lastl, time); + } + lastl = time; switch (disambiguator_step(&d, time, length)) { default: @@ -163,7 +40,8 @@ int main() { { double l = length; char cc = round(l / 500) - 6; - printf("SYNC %s %2d %10li %c%d %10li\n", controller, sensor, time, cc & 0x1 ? 'j' : 'k', (cc >> 1) & 0x3, time-last); + 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; -- cgit v1.2.3 From a7b317f9d16a8fad452be0978d4456d6a2ed2c35 Mon Sep 17 00:00:00 2001 From: Julian Picht Date: Tue, 20 Dec 2016 21:35:54 +0100 Subject: fix typo --- src/survive_data.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/survive_data.c b/src/survive_data.c index fc86d9f..ef35e0e 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -52,7 +52,7 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement * so->d.code = ((le->length+125)/250) - 12; break; case P_SWEEP: - if (so->d.gcode & 1) return; + if (so->d.code & 1) return; ct->lightproc( so, le->sensor_id, so->d.code >> 1, offset, le->timestamp, le->length ); break; } -- cgit v1.2.3 From bbb1ef6a917a3e0b94c8278b3c6643b523aad5c0 Mon Sep 17 00:00:00 2001 From: Julian Picht Date: Tue, 20 Dec 2016 22:26:25 +0100 Subject: add copyright headers --- include/disambiguator.h | 3 +++ src/disambiguator.c | 4 ++++ tools/disambiguate/disambiguate.c | 6 +++++- 3 files changed, 12 insertions(+), 1 deletion(-) diff --git a/include/disambiguator.h b/include/disambiguator.h index 58bce48..a17fc8d 100644 --- a/include/disambiguator.h +++ b/include/disambiguator.h @@ -1,3 +1,6 @@ +// (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. #ifndef DISAMBIGUATOR_H #define DISAMBIGUATOR_H diff --git a/src/disambiguator.c b/src/disambiguator.c index 9b55fa4..f03299d 100644 --- a/src/disambiguator.c +++ b/src/disambiguator.c @@ -1,3 +1,7 @@ +// (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. + #include "../include/disambiguator.h" #include #include diff --git a/tools/disambiguate/disambiguate.c b/tools/disambiguate/disambiguate.c index dcaf9d8..7e46625 100644 --- a/tools/disambiguate/disambiguate.c +++ b/tools/disambiguate/disambiguate.c @@ -1,3 +1,7 @@ +// (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. + #include #include #include @@ -6,7 +10,7 @@ int main() { - FILE * f = fopen( "raw_light_data_from_watchman.csv", "r" ); + FILE * f = fopen( "raw_light_data_from_watchman.sorted.csv", "r" ); if (f == NULL) { fprintf(stderr, "ERROR OPENING INPUT FILE\n"); return -1; -- cgit v1.2.3 From 093802f9cc7ebaa0bfe8d862766e7e08026576f0 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Tue, 20 Dec 2016 22:44:10 -0500 Subject: switch to a hidden disambiguator. --- data_recorder.c | 43 ++++++++++++++++++------------------------- include/disambiguator.h | 38 -------------------------------------- include/survive.h | 3 +-- src/disambiguator.c | 17 ++++++++++------- src/disambiguator.h | 38 ++++++++++++++++++++++++++++++++++++++ src/survive.c | 4 ++++ src/survive_data.c | 10 +++++----- 7 files changed, 76 insertions(+), 77 deletions(-) delete mode 100644 include/disambiguator.h create mode 100644 src/disambiguator.h diff --git a/data_recorder.c b/data_recorder.c index 913f8e0..cdcf4f5 100644 --- a/data_recorder.c +++ b/data_recorder.c @@ -33,51 +33,44 @@ void HandleMotion( int x, int y, int mask ) { } -int bufferpts[32*2]; -char buffermts[32*128]; -int buffertimeto[32]; +int bufferpts[32*2*3]; +char buffermts[32*128*3]; +int buffertimeto[32*3]; void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ) { if( acode == -1 ) return; + int jumpoffset = sensor_id; + if( strcmp( so->codename, "WM0" ) == 0 ) jumpoffset += 32; + else if( strcmp( so->codename, "WM1" ) == 0 ) jumpoffset += 64; + + if( acode == 0 || acode == 2 ) //data = 0 { printf( "L X %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length ); - if( strcmp( so->codename, "HMD" ) == 0 ) - { - bufferpts[sensor_id*2+0] = (timeinsweep-100000)/500; - buffertimeto[sensor_id] = 0; - } + bufferpts[jumpoffset*2+0] = (timeinsweep-100000)/500; + buffertimeto[jumpoffset] = 0; } if( acode == 1 || acode == 3 ) //data = 1 { printf( "L Y %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length ); - if( strcmp( so->codename, "HMD" ) == 0 ) - { - bufferpts[sensor_id*2+1] = (timeinsweep-100000)/500; - buffertimeto[sensor_id] = 0; - } + bufferpts[jumpoffset*2+1] = (timeinsweep-100000)/500; + buffertimeto[jumpoffset] = 0; } if( acode == 4 || acode == 6 ) //data = 0 { printf( "R X %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length ); - if( strcmp( so->codename, "HMD" ) == 0 ) - { - bufferpts[sensor_id*2+0] = (timeinsweep-100000)/500; - buffertimeto[sensor_id] = 0; - } + bufferpts[jumpoffset*2+0] = (timeinsweep-100000)/500; + buffertimeto[jumpoffset] = 0; } if( acode == 5 || acode == 7 ) //data = 1 { printf( "R Y %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length ); - if( strcmp( so->codename, "HMD" ) == 0 ) - { - bufferpts[sensor_id*2+1] = (timeinsweep-100000)/500; - buffertimeto[sensor_id] = 0; - } + bufferpts[jumpoffset*2+1] = (timeinsweep-100000)/500; + buffertimeto[jumpoffset] = 0; } } @@ -105,9 +98,9 @@ void * GuiThread( void * v ) CNFGGetDimensions( &screenx, &screeny ); int i; - for( i = 0; i < 32; i++ ) + for( i = 0; i < 32*3; i++ ) { - if( buffertimeto[i] < 5 ) + if( buffertimeto[i] < 50 ) { uint32_t color = i * 3231349; uint8_t r = color & 0xff; diff --git a/include/disambiguator.h b/include/disambiguator.h deleted file mode 100644 index a17fc8d..0000000 --- a/include/disambiguator.h +++ /dev/null @@ -1,38 +0,0 @@ -// (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. -#ifndef DISAMBIGUATOR_H -#define DISAMBIGUATOR_H - -#define DIS_NUM_VALUES 8 - -typedef enum { - D_STATE_INVALID = 0, - D_STATE_LOCKED = 1, - D_STATE_UNLOCKED = -1, -} dis_state; - -typedef enum { - P_UNKNOWN = 0, - P_SYNC = 1, - P_SWEEP = 2, -} pulse_type; - -typedef struct disambiguator_ { - long times[DIS_NUM_VALUES]; - int scores[DIS_NUM_VALUES]; - dis_state state; - long last; - int max_confidence; - char code; -} disambiguator; - -typedef struct classified_pulse_ { - pulse_type t; - int length; -} classified_pulse; - -void disambiguator_init(disambiguator * d); -pulse_type disambiguator_step(disambiguator * d, long time, int length); - -#endif /* DISAMBIGUATOR_H */ \ No newline at end of file diff --git a/include/survive.h b/include/survive.h index e29c82b..889e24d 100644 --- a/include/survive.h +++ b/include/survive.h @@ -1,7 +1,6 @@ #ifndef _SURVIVE_H #define _SURVIVE_H -#include "disambiguator.h" #include #define SV_FLOAT double @@ -38,7 +37,7 @@ struct SurviveObject int32_t total_photo_time; int32_t total_pulsecode_time; #else - disambiguator d; + struct disambiguator * d; #endif }; diff --git a/src/disambiguator.c b/src/disambiguator.c index f03299d..8a5a993 100644 --- a/src/disambiguator.c +++ b/src/disambiguator.c @@ -2,11 +2,11 @@ // //All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses. -#include "../include/disambiguator.h" +#include "disambiguator.h" #include #include -void disambiguator_init(disambiguator * d) { +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; @@ -14,9 +14,10 @@ void disambiguator_init(disambiguator * d) { d->max_confidence = 0; } -inline void disambiguator_discard(disambiguator * d, long age); +inline void disambiguator_discard( struct disambiguator * d, long age ); -void disambiguator_discard(disambiguator * d, long age) { +void disambiguator_discard( struct disambiguator * d, long age ) +{ int confidence = 0; for (unsigned int i = 0; i < DIS_NUM_VALUES; ++i) { if (d->times[i] != 0 && d->times[i] < age) { @@ -31,9 +32,10 @@ void disambiguator_discard(disambiguator * d, long age) { d->max_confidence = confidence; } -inline int disambiguator_find_nearest(disambiguator * d, long time, int max_diff); +inline int disambiguator_find_nearest( struct disambiguator * d, long time, int max_diff ); -int disambiguator_find_nearest(disambiguator * d, long time, int max_diff) { +int disambiguator_find_nearest( struct disambiguator * d, long time, 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) { @@ -49,7 +51,8 @@ int disambiguator_find_nearest(disambiguator * d, long time, int max_diff) { return idx; } -pulse_type disambiguator_step(disambiguator * d, long time, int length) { +pulse_type disambiguator_step( struct disambiguator * d, long time, int length) +{ if (length < 2750) { return d->state == D_STATE_LOCKED ? P_SWEEP : P_UNKNOWN; } diff --git a/src/disambiguator.h b/src/disambiguator.h new file mode 100644 index 0000000..0db19de --- /dev/null +++ b/src/disambiguator.h @@ -0,0 +1,38 @@ +// (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. +#ifndef DISAMBIGUATOR_H +#define DISAMBIGUATOR_H + +#define DIS_NUM_VALUES 8 + +typedef enum { + D_STATE_INVALID = 0, + D_STATE_LOCKED = 1, + D_STATE_UNLOCKED = -1, +} dis_state; + +typedef enum { + P_UNKNOWN = 0, + P_SYNC = 1, + P_SWEEP = 2, +} pulse_type; + +struct disambiguator { + long times[DIS_NUM_VALUES]; + int scores[DIS_NUM_VALUES]; + dis_state state; + long last; + int max_confidence; + char code; +}; + +struct classified_pulse_ { + pulse_type t; + int length; +}; + +void disambiguator_init( struct disambiguator * d); +pulse_type disambiguator_step( struct disambiguator * d, long time, int length); + +#endif /* DISAMBIGUATOR_H */ diff --git a/src/survive.c b/src/survive.c index b12c4cc..73df29d 100644 --- a/src/survive.c +++ b/src/survive.c @@ -8,6 +8,7 @@ #include #include #include +#include "disambiguator.h" static int jsoneq(const char *json, jsmntok_t *tok, const char *s) { if (tok->type == JSMN_STRING && (int) strlen(s) == tok->end - tok->start && @@ -146,12 +147,15 @@ struct SurviveContext * survive_init() ctx->headset.ctx = ctx; memcpy( ctx->headset.codename, "HMD", 4 ); + ctx->headset.d = calloc( 1, sizeof( struct disambiguator ) ); ctx->watchman[0].ctx = ctx; memcpy( ctx->watchman[0].codename, "WM0", 4 ); + ctx->watchman[0].d = calloc( 1, sizeof( struct disambiguator ) ); ctx->watchman[1].ctx = ctx; memcpy( ctx->watchman[1].codename, "WM1", 4 ); + ctx->watchman[1].d = calloc( 1, sizeof( struct disambiguator ) ); //USB must happen last. if( r = survive_usb_init( ctx ) ) diff --git a/src/survive_data.c b/src/survive_data.c index ef35e0e..cb3e8a2 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -42,18 +42,18 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement * if( le->type != 0xfe || le->length < 50 ) return; //le->timestamp += (le->length/2); - int32_t offset = le->timestamp - so->d.last; - switch(disambiguator_step(&(so->d), le->timestamp, le->length)) { + int32_t offset = le->timestamp - so->d->last; + switch( disambiguator_step( so->d, le->timestamp, le->length ) ) { default: case P_UNKNOWN: // not currently locked case P_SYNC: ct->lightproc( so, le->sensor_id, -1, 0, le->timestamp, offset ); - so->d.code = ((le->length+125)/250) - 12; + so->d->code = ((le->length+125)/250) - 12; break; case P_SWEEP: - if (so->d.code & 1) return; - ct->lightproc( so, le->sensor_id, so->d.code >> 1, offset, le->timestamp, le->length ); + if (so->d->code & 1) return; + ct->lightproc( so, le->sensor_id, so->d->code >> 1, offset, le->timestamp, le->length ); break; } #if 0 -- cgit v1.2.3 From 170219fe413602508b31c39df23c8938adc6c35f Mon Sep 17 00:00:00 2001 From: cnlohr Date: Tue, 20 Dec 2016 23:36:55 -0500 Subject: get the new disambiguator closer, but it's still not perfect. --- Makefile | 2 +- data_recorder.c | 3 ++- include/survive.h | 2 +- src/disambiguator.c | 5 ++++- src/survive.c | 12 ++++++------ src/survive_data.c | 5 +++-- src/survive_usb.c | 12 ++++++------ 7 files changed, 23 insertions(+), 18 deletions(-) diff --git a/Makefile b/Makefile index 90e0ec1..8279efa 100644 --- a/Makefile +++ b/Makefile @@ -1,6 +1,6 @@ all : lib data_recorder test -CFLAGS:=-Iinclude -fPIC -g -Os -Iredist -flto +CFLAGS:=-Iinclude -fPIC -g -Os -Iredist -flto -DUSE_OLD_DISAMBIGUATOR LDFLAGS:=-lpthread -lusb-1.0 -lz -lX11 -flto test : test.c lib/libsurvive.so redist/os_generic.o diff --git a/data_recorder.c b/data_recorder.c index cdcf4f5..4c5627f 100644 --- a/data_recorder.c +++ b/data_recorder.c @@ -40,7 +40,7 @@ int buffertimeto[32*3]; void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ) { if( acode == -1 ) return; - +//return; int jumpoffset = sensor_id; if( strcmp( so->codename, "WM0" ) == 0 ) jumpoffset += 32; else if( strcmp( so->codename, "WM1" ) == 0 ) jumpoffset += 64; @@ -77,6 +77,7 @@ void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int void my_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ) { +return; //if( so->codename[0] == 'H' ) if( 1 ) { diff --git a/include/survive.h b/include/survive.h index 889e24d..8ce5404 100644 --- a/include/survive.h +++ b/include/survive.h @@ -31,7 +31,7 @@ struct SurviveObject //Flood info, for calculating which laser is currently sweeping. int8_t oldcode; -#if 0 +#ifdef USE_OLD_DISAMBIGUATOR int16_t total_photos; int32_t last_photo_time; int32_t total_photo_time; diff --git a/src/disambiguator.c b/src/disambiguator.c index 8a5a993..f641834 100644 --- a/src/disambiguator.c +++ b/src/disambiguator.c @@ -56,10 +56,13 @@ pulse_type disambiguator_step( struct disambiguator * d, long time, int length) 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); - + if (time - d->last > 401000) { d->state = D_STATE_UNLOCKED; } diff --git a/src/survive.c b/src/survive.c index 73df29d..7ad2ae9 100644 --- a/src/survive.c +++ b/src/survive.c @@ -92,12 +92,12 @@ static int LoadConfig( struct SurviveContext * ctx, struct SurviveObject * so, i int i; int r = jsmn_parse(&p, ct0conf, len, t, sizeof(t)/sizeof(t[0])); if (r < 0) { - SV_ERROR("Failed to parse JSON in HMD configuration: %d\n", r); - return 0; + SV_INFO("Failed to parse JSON in HMD configuration: %d\n", r); + return -1; } if (r < 1 || t[0].type != JSMN_OBJECT) { - SV_ERROR("Object expected in HMD configuration\n"); - return 0; + SV_INFO("Object expected in HMD configuration\n"); + return -2; } for (i = 1; i < r; i++) { @@ -166,8 +166,8 @@ struct SurviveContext * survive_init() //Next, pull out the config stuff. if( LoadConfig( ctx, &ctx->headset, 1, 0, 0 ) ) goto fail_gracefully; - if( LoadConfig( ctx, &ctx->watchman[0], 2, 0, 1 ) ) goto fail_gracefully; - if( LoadConfig( ctx, &ctx->watchman[1], 3, 0, 1 ) ) goto fail_gracefully; + if( LoadConfig( ctx, &ctx->watchman[0], 2, 0, 1 ) ) { SV_INFO( "Watchman 0 config issue." ); } + if( LoadConfig( ctx, &ctx->watchman[1], 3, 0, 1 ) ) { SV_INFO( "Watchman 1 config issue." ); } /* int i; diff --git a/src/survive_data.c b/src/survive_data.c index cb3e8a2..6a1656b 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -41,7 +41,7 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement * if( le->type != 0xfe || le->length < 50 ) return; //le->timestamp += (le->length/2); - +#if 0 int32_t offset = le->timestamp - so->d->last; switch( disambiguator_step( so->d, le->timestamp, le->length ) ) { default: @@ -56,7 +56,8 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement * ct->lightproc( so, le->sensor_id, so->d->code >> 1, offset, le->timestamp, le->length ); break; } -#if 0 +#endif +#ifdef USE_OLD_DISAMBIGUATOR if( le->length > 2100 ) //Pulse longer indicates a sync pulse. { int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_photo_time; diff --git a/src/survive_usb.c b/src/survive_usb.c index 808236d..cf12280 100644 --- a/src/survive_usb.c +++ b/src/survive_usb.c @@ -384,7 +384,7 @@ int survive_get_config( char ** config, struct SurviveContext * ctx, int devno, cfgbuff[0] = 0x10; if( ( ret = hid_get_feature_report_timeout( dev, iface, cfgbuff, sizeof( cfgbuff ) ) ) < 0 ) { - SV_ERROR( "Could not get survive config data for device %d:%d", devno, iface ); + SV_INFO( "Could not get survive config data for device %d:%d", devno, iface ); return -1; } @@ -395,7 +395,7 @@ int survive_get_config( char ** config, struct SurviveContext * ctx, int devno, { if( (ret = hid_get_feature_report_timeout(dev, iface, cfgbuff, sizeof( cfgbuff ) ) ) < 0 ) { - SV_ERROR( "Could not read config data (after first packet) on device %d:%d (count: %d)\n", devno, iface, count ); + SV_INFO( "Could not read config data (after first packet) on device %d:%d (count: %d)\n", devno, iface, count ); return -2; } @@ -405,13 +405,13 @@ int survive_get_config( char ** config, struct SurviveContext * ctx, int devno, if( size > 62 ) { - SV_ERROR( "Too much data (%d) on packet from config for device %d:%d (count: %d)", size, devno, iface, count ); + SV_INFO( "Too much data (%d) on packet from config for device %d:%d (count: %d)", size, devno, iface, count ); return -3; } if( count + size >= sizeof( compressed_data ) ) { - SV_ERROR( "Configuration length too long %d:%d (count: %d)", devno, iface, count ); + SV_INFO( "Configuration length too long %d:%d (count: %d)", devno, iface, count ); return -4; } @@ -421,7 +421,7 @@ int survive_get_config( char ** config, struct SurviveContext * ctx, int devno, if( count == 0 ) { - SV_ERROR( "Empty configuration for %d:%d", devno, iface ); + SV_INFO( "Empty configuration for %d:%d", devno, iface ); return -5; } @@ -430,7 +430,7 @@ int survive_get_config( char ** config, struct SurviveContext * ctx, int devno, int len = survive_simple_inflate( ctx, compressed_data, count, uncompressed_data, sizeof(uncompressed_data)-1 ); if( len <= 0 ) { - SV_ERROR( "Error: data for config descriptor %d:%d is bad.", devno, iface ); + SV_INFO( "Error: data for config descriptor %d:%d is bad.", devno, iface ); return -5; } -- cgit v1.2.3