From 44d7e56bf6a419104fee5dd2aedc26e00e543d69 Mon Sep 17 00:00:00 2001 From: Charles Lohr Date: Sun, 11 Dec 2016 00:03:40 -0500 Subject: * Get config from headset. * Make pretty display. * Improve disambiguation. --- src/survive.c | 82 +++++++++++++++++++++++++++++++++++++++++++++++--- src/survive_data.c | 9 ++++-- src/survive_internal.h | 5 +++ src/survive_process.c | 32 ++++++++++++++++++-- src/survive_usb.c | 19 +++++------- 5 files changed, 125 insertions(+), 22 deletions(-) (limited to 'src') diff --git a/src/survive.c b/src/survive.c index 726e237..b5c8012 100644 --- a/src/survive.c +++ b/src/survive.c @@ -5,9 +5,17 @@ #include "survive_internal.h" #include #include +#include #include #include +static int jsoneq(const char *json, jsmntok_t *tok, const char *s) { + if (tok->type == JSMN_STRING && (int) strlen(s) == tok->end - tok->start && + strncmp(json + tok->start, s, tok->end - tok->start) == 0) { + return 0; + } + return -1; +} struct SurviveContext * survive_init( void(*ff)( struct SurviveContext * ctx, const char * fault ), void(*notefunction)( struct SurviveContext * ctx, const char * note ) ) { @@ -32,15 +40,79 @@ struct SurviveContext * survive_init( void(*ff)( struct SurviveContext * ctx, co //USB must happen last. if( r = survive_usb_init( ctx ) ) { + //TODO: Cleanup any libUSB stuff sitting around. return 0; } -#if 0 +#if 1 //Next, pull out the config stuff. - char * ct0conf; - int len = survive_get_config( &ct0conf, ctx, 0, 0 ); - printf( "%d\n", len ); - puts( ct0conf ); + { + char * ct0conf = 0; + int len = survive_get_config( &ct0conf, ctx, 1, 0 ); + if( len > 0 ) + { + //From JSMN example. + jsmn_parser p; + jsmntok_t t[4096]; + jsmn_init(&p); + 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; + } + if (r < 1 || t[0].type != JSMN_OBJECT) { + SV_ERROR("Object expected in HMD configuration\n"); + return 0; + } + for (i = 1; i < r; i++) { + if (jsoneq(ct0conf, &t[i], "modelPoints") == 0) { + int k; + jsmntok_t * tk = &t[i+1]; + //printf( "%d / %d / %d / %d\n", tk->type, tk->start, tk->end, tk->size ); + int pts = tk->size; + + ctx->headset.nr_locations = 0; + ctx->headset.sensor_locations = malloc( sizeof( *ctx->headset.sensor_locations) * 32 * 3 ); + + for( k = 0; k < pts; k++ ) + { + tk = &t[i+2+k*4]; + //printf( "++%d / %d / %d / %d\n", tk->type, tk->start, tk->end, tk->size ); + + float vals[3]; + int m; + for( m = 0; m < 3; m++ ) + { + char ctt[128]; + + tk++; + int elemlen = tk->end - tk->start; + + if( tk->type != 4 || elemlen > sizeof( ctt )-1 ) + { + SV_ERROR( "Parse error in JSON\n" ); + break; + } + + memcpy( ctt, ct0conf + tk->start, elemlen ); + ctt[elemlen] = 0; + float f = atof( ctt ); + int id = ctx->headset.nr_locations*3+m; + ctx->headset.sensor_locations[id] = f; + } + ctx->headset.nr_locations++; + } + } + } + } + else + { + //TODO: Cleanup any remaining USB stuff. + return 0; + } + + } #endif //ctx->headset->photos = malloc( ctx->headset->sensors * sizeof(struct SurvivePhoto) ); diff --git a/src/survive_data.c b/src/survive_data.c index f5d0e6b..11677e0 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -35,7 +35,8 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement * if( le->type != 0xfe || le->length < 50 ) return; //le->timestamp += (le->length/2); - if( le->length > 900 ) //Pulse longer than 18us? + + if( le->length > 2100 ) //Pulse longer indicates a sync pulse. { int32_t deltat = (uint32_t)le->timestamp - (uint32_t)ct->last_photo_time; if( deltat > 2000 || deltat < -2000 ) //New pulse. (may be inverted) @@ -65,8 +66,12 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement * int32_t acode = (tpco+125)/250; if( acode & 1 ) return; acode>>=1; + acode -= 6; - survive_light_process( so, le->sensor_id, acode, offset_from, le->timestamp ); + if( offset_from < 380000 ) + { + survive_light_process( so, le->sensor_id, acode, offset_from, le->timestamp ); + } } else { diff --git a/src/survive_internal.h b/src/survive_internal.h index 8096b8c..d6168b2 100644 --- a/src/survive_internal.h +++ b/src/survive_internal.h @@ -26,6 +26,8 @@ //XXX TODO This one needs to be rewritten. #define SV_KILL() exit(0) +#define SV_FLOAT double + #define USB_DEV_HMD 0 #define USB_DEV_LIGHTHOUSE 1 #define USB_DEV_WATCHMAN1 2 @@ -72,6 +74,9 @@ struct SurviveObject int8_t charging:1; int8_t ison:1; int sensors; + + int nr_locations; + SV_FLOAT * sensor_locations; }; struct SurviveContext diff --git a/src/survive_process.c b/src/survive_process.c index a4680af..139ae2f 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -3,16 +3,42 @@ #include "survive_internal.h" + +int bufferpts[32*2]; +char buffermts[32*128]; +int buffertimeto[32]; + void survive_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode ) { + if( acode == -1 ) return; + + if( acode == 0 || acode == 2 ) //data = 0 + { + printf( "L, X, %s, %d, %d, %d, %d\n", so->codename, timecode, sensor_id, acode, timeinsweep ); + bufferpts[sensor_id*2+0] = (timeinsweep-170000)/100; + buffertimeto[sensor_id] = 0; + //printf( "X: %d\n",bufferpts[sensor_id*2+0] ); + //480-(timeinsweep)/1000; // Full scan + } + if( acode == 1 || acode == 3 ) //data = 1 + { + printf( "L, Y, %s, %d, %d, %d, %d\n", so->codename, timecode, sensor_id, acode, timeinsweep ); + bufferpts[sensor_id*2+1] = 480-(timeinsweep-140000)/100; + //printf( "Y: %d\n",bufferpts[sensor_id*2+1] ); + buffertimeto[sensor_id] = 0; + + //480-(timeinsweep)/1000; //Full scan + } + + //timeinsweep = 200,000 1/48,000,000ths of a second is "center-of-image" - //printf( "L %s %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep ); } void survive_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ) { - if( so->codename[0] != 'H' ) + //if( so->codename[0] == 'H' ) + if( 0 ) { - //printf( "I %s %d %d %d %d %d %d %d %d\n", so->codename, timecode, accelgyro[0], accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id ); + printf( "I %s %d %d %d %d %d %d %d %d\n", so->codename, timecode, accelgyro[0], accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id ); } } diff --git a/src/survive_usb.c b/src/survive_usb.c index cfcef17..affcdd5 100644 --- a/src/survive_usb.c +++ b/src/survive_usb.c @@ -298,8 +298,7 @@ int survive_usb_poll( struct SurviveContext * ctx ) return r; } -#if 0 -//XXX THIS DOES NOT WORK!!! WHY??? + int survive_get_config( char ** config, struct SurviveContext * ctx, int devno, int iface ) { int i, ret, count = 0, size = 0; @@ -327,10 +326,10 @@ int survive_get_config( char ** config, struct SurviveContext * ctx, int devno, } size = cfgbuff[1]; - printf( "Tag: " ); - for( i = 0; i < 64; i++ ) - printf( "%02x ", cfgbuff[i] ); - printf( "ret: %d %d\n", ret, size ); +// printf( "Tag: " ); +// for( i = 0; i < 64; i++ ) +// printf( "%02x ", cfgbuff[i] ); +// printf( "ret: %d %d\n", ret, size ); if( !size ) break; @@ -365,13 +364,9 @@ int survive_get_config( char ** config, struct SurviveContext * ctx, int devno, return -5; } - config = malloc( len + 1 ); - memcpy( config, uncompressed_data, len ); + *config = malloc( len + 1 ); + memcpy( *config, uncompressed_data, len ); return len; } -#endif - - - -- cgit v1.2.3