From d4ccaa85963c08e7095f49a0378dc21fdf701209 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 17 Dec 2016 16:58:57 -0500 Subject: Fix light disambiguator location, put with the object instead of the system. --- src/survive.c | 194 ++++++++++++++++++++++++++++++------------------- src/survive_data.c | 37 ++++++---- src/survive_internal.h | 8 -- 3 files changed, 140 insertions(+), 99 deletions(-) (limited to 'src') diff --git a/src/survive.c b/src/survive.c index 773f02a..09c9ddb 100644 --- a/src/survive.c +++ b/src/survive.c @@ -29,6 +29,98 @@ static void survivenote( struct SurviveContext * ctx, const char * fault ) fprintf( stderr, "Info: %s\n", fault ); } +static int ParsePoints( struct SurviveContext * ctx, char * ct0conf, SV_FLOAT ** floats_out, jsmntok_t * t, int i ) +{ + int k; + int pts = t[i+1].size; + jsmntok_t * tk; + + ctx->headset.nr_locations = 0; + *floats_out = malloc( sizeof( **floats_out ) * 32 * 3 ); + + for( k = 0; k < pts; k++ ) + { + tk = &t[i+2+k*4]; + + 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" ); + return 1; + } + + memcpy( ctt, ct0conf + tk->start, elemlen ); + ctt[elemlen] = 0; + float f = atof( ctt ); + int id = ctx->headset.nr_locations*3+m; + (*floats_out)[id] = f; + } + ctx->headset.nr_locations++; + } + return 0; +} + +static int LoadConfig( struct SurviveContext * ctx, struct SurviveObject * so, int devno, int iface ) +{ + char * ct0conf = 0; + int len = survive_get_config( &ct0conf, ctx, devno, iface ); + 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++) { + jsmntok_t * tk = &t[i]; + + char ctxo[100]; + int ilen = tk->end - tk->start; + if( ilen > 99 ) ilen = 99; + memcpy(ctxo, ct0conf + tk->start, ilen); + ctxo[ilen] = 0; +// printf( "%d / %d / %d / %d %s %d\n", tk->type, tk->start, tk->end, tk->size, ctxo, jsoneq(ct0conf, &t[i], "modelPoints") ); +// printf( "%.*s\n", ilen, ct0conf + tk->start ); + if (jsoneq(ct0conf, tk, "modelPoints") == 0) { + if( ParsePoints( ctx, ct0conf, &ctx->headset.sensor_locations, t, i ) ) + { + break; + } + } + if (jsoneq(ct0conf, tk, "modelNormals") == 0) { + if( ParsePoints( ctx, ct0conf, &ctx->headset.sensor_normals, t, i ) ) + { + break; + } + } + } + } + else + { + //TODO: Cleanup any remaining USB stuff. + return 1; + } + return 0; +} struct SurviveContext * survive_init() { @@ -43,103 +135,55 @@ struct SurviveContext * survive_init() ctx->headset.sensors = 32; ctx->headset.ctx = ctx; - memcpy( ctx->headset.codename, "HED", 4 ); + memcpy( ctx->headset.codename, "HMD", 4 ); ctx->watchman[0].sensors = 16; ctx->watchman[0].ctx = ctx; - memcpy( ctx->watchman[0].codename, "CT0", 4 ); + memcpy( ctx->watchman[0].codename, "WM0", 4 ); ctx->watchman[1].sensors = 16; ctx->watchman[1].ctx = ctx; - memcpy( ctx->watchman[1].codename, "CT1", 4 ); + memcpy( ctx->watchman[1].codename, "WM1", 4 ); //USB must happen last. if( r = survive_usb_init( ctx ) ) { //TODO: Cleanup any libUSB stuff sitting around. - return 0; + goto fail_gracefully; } //Next, pull out the config stuff. + if( LoadConfig( ctx, &ctx->headset, 1, 0 ) ) goto fail_gracefully; + +/* + int i; + int locs = ctx->headset.nr_locations; + printf( "Locs: %d\n", locs ); + if (ctx->headset.sensor_locations ) { - char * ct0conf = 0; - int len = survive_get_config( &ct0conf, ctx, 1, 0 ); - if( len > 0 ) + printf( "POSITIONS:\n" ); + for( i = 0; i < locs*3; i+=3 ) { - - //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++) { - jsmntok_t * tk = &t[i]; - - char ctxo[100]; - int ilen = tk->end - tk->start; - if( ilen > 99 ) ilen = 99; - memcpy(ctxo, ct0conf + tk->start, ilen); - ctxo[ilen] = 0; -// printf( "%d / %d / %d / %d %s %d\n", tk->type, tk->start, tk->end, tk->size, ctxo, jsoneq(ct0conf, &t[i], "modelPoints") ); - - if (jsoneq(ct0conf, &t[i], "modelPoints") == 0) { - int k; - int pts = t[i+1].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]; - - 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 ); - //printf( "%f%c", f, (m==2)?'\n':',' ); - int id = ctx->headset.nr_locations*3+m; - ctx->headset.sensor_locations[id] = f; - } - ctx->headset.nr_locations++; - } - } - } + printf( "%f %f %f\n", ctx->headset.sensor_locations[i+0], ctx->headset.sensor_locations[i+1], ctx->headset.sensor_locations[i+2] ); } - else + } + if( ctx->headset.sensor_normals ) + { + printf( "NORMALS:\n" ); + for( i = 0; i < locs*3; i+=3 ) { - //TODO: Cleanup any remaining USB stuff. - return 0; + printf( "%f %f %f\n", ctx->headset.sensor_normals[i+0], ctx->headset.sensor_normals[i+1], ctx->headset.sensor_normals[i+2] ); } - } +*/ + + - //ctx->headset->photos = malloc( ctx->headset->sensors * sizeof(struct SurvivePhoto) ); return ctx; +fail_gracefully: + survive_usb_close( ctx ); + free( ctx ); + return 0; } void survive_install_info_fn( struct SurviveContext * ctx, text_feedback_fnptr fbp ) diff --git a/src/survive_data.c b/src/survive_data.c index 88c9e7d..660c3e1 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -28,41 +28,46 @@ struct LightcapElement } __attribute__((packed)); - +//This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode. static void handle_lightcap( struct SurviveObject * so, struct LightcapElement * le ) { struct SurviveContext * ct = so->ctx; +// if( so->codename[0] == 'W' ) +// { +// printf( "%s %d %d %d %d\n", so->codename, le->sensor_id, le->type, le->length, le->timestamp ); +// } + if( le->type != 0xfe || le->length < 50 ) return; //le->timestamp += (le->length/2); if( le->length > 2100 ) //Pulse longer indicates a sync pulse. { - int32_t deltat = (uint32_t)le->timestamp - (uint32_t)ct->last_photo_time; + int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_photo_time; if( deltat > 2000 || deltat < -2000 ) //New pulse. (may be inverted) { - if( le->timestamp - ct->last_photo_time > 80000 ) + if( le->timestamp - so->last_photo_time > 80000 ) { - ct->last_photo_time = le->timestamp; - ct->total_photo_time = 0; - ct->total_photos = 0; - ct->total_pulsecode_time = 0; + so->last_photo_time = le->timestamp; + so->total_photo_time = 0; + so->total_photos = 0; + so->total_pulsecode_time = 0; ct->lightproc( so, le->sensor_id, -1, 0, le->timestamp, deltat ); } } else { - ct->total_pulsecode_time += le->length; - ct->total_photo_time += deltat; - ct->total_photos++; + so->total_pulsecode_time += le->length; + so->total_photo_time += deltat; + so->total_photos++; } } - else if( le->length < 1200 && le->length > 40 && ct->total_photos ) + else if( le->length < 1200 && le->length > 40 && so->total_photos ) { - int32_t dl = (ct->total_photo_time/ct->total_photos); - int32_t tpco = (ct->total_pulsecode_time/ct->total_photos); + int32_t dl = (so->total_photo_time/so->total_photos); + int32_t tpco = (so->total_pulsecode_time/so->total_photos); //Adding length - int32_t offset_from = le->timestamp - dl - ct->last_photo_time + le->length/2; + int32_t offset_from = le->timestamp - dl - so->last_photo_time + le->length/2; //Long pulse-code from IR flood. //Make sure it fits nicely into a divisible-by-500 time. @@ -341,11 +346,11 @@ void survive_data_cb( struct SurviveUSBInterface * si ) uint32_t timecode = POP4; uint8_t code = POP1; //printf( "%d ", code ); - int8_t cd = code - ctx->oldcode; + int8_t cd = code - ctx->headset.oldcode; if( cd > 0 ) { - ctx->oldcode = code; + ctx->headset.oldcode = code; ctx->imuproc( &ctx->headset, acceldata, timecode, code ); } } diff --git a/src/survive_internal.h b/src/survive_internal.h index ac378a2..92a2001 100644 --- a/src/survive_internal.h +++ b/src/survive_internal.h @@ -75,17 +75,9 @@ struct SurviveContext light_process_func lightproc; imu_process_func imuproc; - //Flood info, for calculating which laser is currently sweeping. - int8_t oldcode; - int32_t last_photo_time; - short total_photos; - int32_t total_photo_time; - int32_t total_pulsecode_time; - //Data Subsystem struct SurviveObject headset; struct SurviveObject watchman[2]; -// struct SurvivePhoto }; -- cgit v1.2.3