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. --- data_recorder.c | 8 +- include/survive.h | 21 +++++- src/survive.c | 194 ++++++++++++++++++++++++++++++------------------- src/survive_data.c | 37 ++++++---- src/survive_internal.h | 8 -- 5 files changed, 161 insertions(+), 107 deletions(-) diff --git a/data_recorder.c b/data_recorder.c index 24c8bc1..913f8e0 100644 --- a/data_recorder.c +++ b/data_recorder.c @@ -44,7 +44,7 @@ void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int if( acode == 0 || acode == 2 ) //data = 0 { printf( "L X %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length ); - if( strcmp( so->codename, "HED" ) == 0 ) + if( strcmp( so->codename, "HMD" ) == 0 ) { bufferpts[sensor_id*2+0] = (timeinsweep-100000)/500; buffertimeto[sensor_id] = 0; @@ -53,7 +53,7 @@ void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int if( acode == 1 || acode == 3 ) //data = 1 { printf( "L Y %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length ); - if( strcmp( so->codename, "HED" ) == 0 ) + if( strcmp( so->codename, "HMD" ) == 0 ) { bufferpts[sensor_id*2+1] = (timeinsweep-100000)/500; buffertimeto[sensor_id] = 0; @@ -64,7 +64,7 @@ void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int if( acode == 4 || acode == 6 ) //data = 0 { printf( "R X %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length ); - if( strcmp( so->codename, "HED" ) == 0 ) + if( strcmp( so->codename, "HMD" ) == 0 ) { bufferpts[sensor_id*2+0] = (timeinsweep-100000)/500; buffertimeto[sensor_id] = 0; @@ -73,7 +73,7 @@ void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int if( acode == 5 || acode == 7 ) //data = 1 { printf( "R Y %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length ); - if( strcmp( so->codename, "HED" ) == 0 ) + if( strcmp( so->codename, "HMD" ) == 0 ) { bufferpts[sensor_id*2+1] = (timeinsweep-100000)/500; buffertimeto[sensor_id] = 0; diff --git a/include/survive.h b/include/survive.h index 61a6620..b574711 100644 --- a/include/survive.h +++ b/include/survive.h @@ -7,23 +7,36 @@ struct SurviveContext; -//DANGER: This structure may be redefined +//DANGER: This structure may be redefined. Note that it is logically split into 64-bit chunks +//for optimization on 32- and 64-bit systems. struct SurviveObject { struct SurviveContext * ctx; - char codename[4]; + + char codename[4]; //3 letters, null-terminated. Currently HMD, WM0, WM1. int16_t buttonmask; int16_t axis1; + int16_t axis2; int16_t axis3; int8_t charge; int8_t charging:1; int8_t ison:1; - int sensors; + int8_t additional_flags:6; + int8_t sensors; + int8_t nr_locations; - int nr_locations; SV_FLOAT * sensor_locations; + + SV_FLOAT * sensor_normals; + + //Flood info, for calculating which laser is currently sweeping. + int32_t last_photo_time; + int32_t total_photo_time; + int32_t total_pulsecode_time; + int16_t total_photos; + int8_t oldcode; }; typedef void (*text_feedback_fnptr)( struct SurviveContext * ctx, const char * fault ); 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