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