From bc65f5ce142ed203e91d8b35a1e61009ef4fb098 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Mon, 19 Dec 2016 02:00:08 -0500 Subject: allow for easy config dumping --- include/survive.h | 10 ++++------ src/survive.c | 35 +++++++++++++++++++++++++---------- test.c | 32 ++++++++++++++++++++++++++++++++ 3 files changed, 61 insertions(+), 16 deletions(-) diff --git a/include/survive.h b/include/survive.h index b574711..f64091f 100644 --- a/include/survive.h +++ b/include/survive.h @@ -24,19 +24,17 @@ struct SurviveObject int8_t charging:1; int8_t ison:1; int8_t additional_flags:6; - int8_t sensors; - int8_t nr_locations; SV_FLOAT * sensor_locations; - SV_FLOAT * sensor_normals; + int8_t nr_locations; //Flood info, for calculating which laser is currently sweeping. + int8_t oldcode; + int16_t total_photos; 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 ); @@ -54,7 +52,7 @@ void survive_install_imu_fn( struct SurviveContext * ctx, imu_process_func fbp void survive_close( struct SurviveContext * ctx ); int survive_poll(); - +struct SurviveObject * survive_get_so_by_name( struct SurviveContext * ctx, const char * name ); //Utilitiy functions. int survive_simple_inflate( struct SurviveContext * ctx, const char * input, int inlen, char * output, int outlen ); diff --git a/src/survive.c b/src/survive.c index 0705801..b12c4cc 100644 --- a/src/survive.c +++ b/src/survive.c @@ -29,13 +29,13 @@ 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 ) +static int ParsePoints( struct SurviveContext * ctx, struct SurviveObject * so, 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; + so->nr_locations = 0; *floats_out = malloc( sizeof( **floats_out ) * 32 * 3 ); for( k = 0; k < pts; k++ ) @@ -60,10 +60,10 @@ static int ParsePoints( struct SurviveContext * ctx, char * ct0conf, SV_FLOAT ** memcpy( ctt, ct0conf + tk->start, elemlen ); ctt[elemlen] = 0; float f = atof( ctt ); - int id = ctx->headset.nr_locations*3+m; + int id = so->nr_locations*3+m; (*floats_out)[id] = f; } - ctx->headset.nr_locations++; + so->nr_locations++; } return 0; } @@ -72,6 +72,15 @@ static int LoadConfig( struct SurviveContext * ctx, struct SurviveObject * so, i { char * ct0conf = 0; int len = survive_get_config( &ct0conf, ctx, devno, iface, extra_magic ); + +#if 0 + char fname[100]; + sprintf( fname, "%s_config.json", so->codename ); + FILE * f = fopen( fname, "w" ); + fwrite( ct0conf, strlen(ct0conf), 1, f ); + fclose( f ); +#endif + if( len > 0 ) { @@ -98,16 +107,18 @@ static int LoadConfig( struct SurviveContext * ctx, struct SurviveObject * so, i 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 ) ) + if( ParsePoints( ctx, so, ct0conf, &so->sensor_locations, t, i ) ) { break; } } if (jsoneq(ct0conf, tk, "modelNormals") == 0) { - if( ParsePoints( ctx, ct0conf, &ctx->headset.sensor_normals, t, i ) ) + if( ParsePoints( ctx, so, ct0conf, &so->sensor_normals, t, i ) ) { break; } @@ -133,15 +144,12 @@ struct SurviveContext * survive_init() ctx->lightproc = survive_default_light_process; ctx->imuproc = survive_default_imu_process; - ctx->headset.sensors = 32; ctx->headset.ctx = ctx; memcpy( ctx->headset.codename, "HMD", 4 ); - ctx->watchman[0].sensors = 16; ctx->watchman[0].ctx = ctx; memcpy( ctx->watchman[0].codename, "WM0", 4 ); - ctx->watchman[1].sensors = 16; ctx->watchman[1].ctx = ctx; memcpy( ctx->watchman[1].codename, "WM1", 4 ); @@ -244,7 +252,6 @@ int survive_simple_inflate( struct SurviveContext * ctx, const char * input, int if( inflate( &zs, Z_FINISH) != Z_STREAM_END ) { - printf( "Zissue\n" ); SV_INFO("survive_simple_inflate could not inflate." ); return -1; } @@ -253,3 +260,11 @@ int survive_simple_inflate( struct SurviveContext * ctx, const char * input, int return len; } +struct SurviveObject * survive_get_so_by_name( struct SurviveContext * ctx, const char * name ) +{ + if( strcmp( name, "HMD" ) == 0 ) return &ctx->headset; + if( strcmp( name, "WM0" ) == 0 ) return &ctx->watchman[0]; + if( strcmp( name, "WM1" ) == 0 ) return &ctx->watchman[1]; + return 0; +} + diff --git a/test.c b/test.c index 537b2e5..17fa7f3 100644 --- a/test.c +++ b/test.c @@ -9,6 +9,34 @@ struct SurviveContext * ctx; + + +static void dump_iface( struct SurviveObject * so, const char * prefix ) +{ + int i; + FILE * f; + char fname[1024]; + + sprintf( fname, "%s_points.csv", prefix ); + f = fopen( fname, "w" ); + for( i = 0; i < so->nr_locations; i++ ) + { + fprintf( f, "%g %g %g\n", so->sensor_locations[i*3+0], so->sensor_locations[i*3+1], so->sensor_locations[i*3+2] ); + } + fclose( f ); + + sprintf( fname, "%s_normals.csv", prefix ); + f = fopen( fname, "w" ); + for( i = 0; i < so->nr_locations; i++ ) + { + fprintf( f, "%g %g %g\n", so->sensor_normals[i*3+0], so->sensor_normals[i*3+1], so->sensor_normals[i*3+2] ); + } + fclose( f ); + +} + + + int main() { int magicon = 0; @@ -22,6 +50,10 @@ int main() return 1; } + dump_iface( survive_get_so_by_name( ctx, "HMD" ), "HMD" ); + dump_iface( survive_get_so_by_name( ctx, "WM0" ), "WM0" ); + dump_iface( survive_get_so_by_name( ctx, "WM1" ), "WM1" ); + while(survive_poll(ctx) == 0) { double Now = OGGetAbsoluteTime(); -- cgit v1.2.3