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. --- Makefile | 7 +++-- redist/os_generic.c | 2 +- src/survive.c | 82 +++++++++++++++++++++++++++++++++++++++++++++++--- src/survive_data.c | 9 ++++-- src/survive_internal.h | 5 +++ src/survive_process.c | 32 ++++++++++++++++++-- src/survive_usb.c | 19 +++++------- test.c | 64 +++++++++++++++++++++++++++++++++++++++ 8 files changed, 194 insertions(+), 26 deletions(-) diff --git a/Makefile b/Makefile index 77aee01..29c05a1 100644 --- a/Makefile +++ b/Makefile @@ -1,12 +1,13 @@ all : test -CFLAGS:=-Iinclude -fPIC -g -Os -LDFLAGS:=-lpthread -lusb-1.0 -lz +CFLAGS:=-Iinclude -fPIC -g -Os -Iredist +LDFLAGS:=-lpthread -lusb-1.0 -lz -lX11 -lXinerama +DEBUGSTUFF:=redist/os_generic.o redist/DrawFunctions.o redist/XDriver.o test : test.c lib/libsurvive.so gcc -o $@ $^ $(LDFLAGS) $(CFLAGS) -lib/libsurvive.so : src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o +lib/libsurvive.so : src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o redist/jsmn.o $(DEBUGSTUFF) gcc -o $@ $^ $(LDFLAGS) -shared clean : diff --git a/redist/os_generic.c b/redist/os_generic.c index 7a5af86..0993d7a 100644 --- a/redist/os_generic.c +++ b/redist/os_generic.c @@ -1,6 +1,5 @@ #include "os_generic.h" - #ifdef USE_WINDOWS #include @@ -159,6 +158,7 @@ void OGDeleteSema( og_sema_t os ) #include #include #include +#include pthread_mutex_t g_RawMutexStart = PTHREAD_MUTEX_INITIALIZER; 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 - - - diff --git a/test.c b/test.c index 8017872..5eff185 100644 --- a/test.c +++ b/test.c @@ -4,6 +4,10 @@ #include #include +#include +#include + + void survivefault( struct SurviveContext * ctx, const char * fault ) { fprintf( stderr, "Error: %s\n", fault ); @@ -15,6 +19,61 @@ void survivenote( struct SurviveContext * ctx, const char * fault ) fprintf( stderr, "Info: %s\n", fault ); } + +void HandleKey( int keycode, int bDown ) +{ +} + +void HandleButton( int x, int y, int button, int bDown ) +{ +} + +void HandleMotion( int x, int y, int mask ) +{ +} + +extern int bufferpts[32*2]; +extern char buffermts[32*128]; +extern int buffertimeto[32]; + +void * GuiThread( void * v ) +{ + short screenx, screeny; + while(1) + { + CNFGHandleInput(); + CNFGClearFrame(); + CNFGColor( 0xFFFFFF ); + CNFGGetDimensions( &screenx, &screeny ); + + int i; + for( i = 0; i < 32; i++ ) + { + if( buffertimeto[i] < 5 ) + { + uint32_t color = i * 3231349; + uint8_t r = color & 0xff; + uint8_t g = (color>>8) & 0xff; + uint8_t b = (color>>16) & 0xff; + r = (r * (5-buffertimeto[i])) / 5 ; + g = (g * (5-buffertimeto[i])) / 5 ; + b = (b * (5-buffertimeto[i])) / 5 ; + CNFGColor( (b<<16) | (g<<8) | r ); + CNFGTackRectangle( bufferpts[i*2+0], bufferpts[i*2+1], bufferpts[i*2+0] + 5, bufferpts[i*2+1] + 5 ); + CNFGPenX = bufferpts[i*2+0]; CNFGPenY = bufferpts[i*2+1]; + CNFGDrawText( buffermts, 2 ); + buffertimeto[i]++; + } + } + + + CNFGSwapBuffers(); + OGUSleep( 10000 ); + } +} + + + int main() { /* int i; @@ -34,6 +93,11 @@ int main() return 0;*/ struct SurviveContext * ctx = survive_init( &survivefault, &survivenote ); + CNFGBGColor = 0x000000; + CNFGDialogColor = 0x444444; + CNFGSetup( "Survive GUI Debug", 640, 480 ); + OGCreateThread( GuiThread, 0 ); + if( !ctx ) { -- cgit v1.2.3