diff options
author | cnlohr <lohr85@gmail.com> | 2016-12-20 23:39:58 -0500 |
---|---|---|
committer | cnlohr <lohr85@gmail.com> | 2016-12-20 23:39:58 -0500 |
commit | 2bce90b5a5095c2327bbbba1d0cc4fdb38426ce0 (patch) | |
tree | a3a955f85472f8888e2837eff821e2dc1f2219e8 /src | |
parent | 0fb1ec7b12f5c0dbe8e82d7d029e900751fc6c6a (diff) | |
parent | 170219fe413602508b31c39df23c8938adc6c35f (diff) | |
download | libsurvive-2bce90b5a5095c2327bbbba1d0cc4fdb38426ce0.tar.gz libsurvive-2bce90b5a5095c2327bbbba1d0cc4fdb38426ce0.tar.bz2 |
Merge branch 'jpicht-disambiguate'
Diffstat (limited to 'src')
-rw-r--r-- | src/disambiguator.c | 93 | ||||
-rw-r--r-- | src/disambiguator.h | 38 | ||||
-rw-r--r-- | src/survive.c | 16 | ||||
-rw-r--r-- | src/survive_data.c | 20 | ||||
-rw-r--r-- | src/survive_usb.c | 12 |
5 files changed, 166 insertions, 13 deletions
diff --git a/src/disambiguator.c b/src/disambiguator.c new file mode 100644 index 0000000..f641834 --- /dev/null +++ b/src/disambiguator.c @@ -0,0 +1,93 @@ +// (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 "disambiguator.h" +#include <stdlib.h> +#include <string.h> + +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; +} + +inline void disambiguator_discard( struct 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) { + 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( struct 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) { + 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( 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; + } + + 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/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..7ad2ae9 100644 --- a/src/survive.c +++ b/src/survive.c @@ -8,6 +8,7 @@ #include <jsmn.h> #include <string.h> #include <zlib.h> +#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 && @@ -91,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++) { @@ -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 ) ) @@ -162,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 660c3e1..6a1656b 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 <stdint.h> #include <string.h> @@ -40,7 +41,23 @@ 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: + 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->code & 1) return; + ct->lightproc( so, le->sensor_id, so->d->code >> 1, offset, le->timestamp, le->length ); + break; + } +#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; @@ -85,6 +102,7 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement * { //Runt pulse. } +#endif } 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; } |