From 5aa1b9214745a625ab644846ada3034e7c38f493 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Thu, 8 Mar 2018 15:08:14 -0700 Subject: Added playback device driver --- src/survive_playback.c | 374 +++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 374 insertions(+) create mode 100755 src/survive_playback.c (limited to 'src/survive_playback.c') diff --git a/src/survive_playback.c b/src/survive_playback.c new file mode 100755 index 0000000..bb66f25 --- /dev/null +++ b/src/survive_playback.c @@ -0,0 +1,374 @@ +//Unofficial driver for the official Valve/HTC Vive hardware. +// +//Based off of https://github.com/collabora/OSVR-Vive-Libre +// Originally Copyright 2016 Philipp Zabel +// Originally Copyright 2016 Lubosz Sarnecki +// Originally Copyright (C) 2013 Fredrik Hultin +// Originally Copyright (C) 2013 Jakob Bornecrantz +// +//But, re-written as best as I can to get it put under an open souce license instead of a forced-source license. +//If there are portions of the code too similar to the original, I would like to know so they can be re-written. +//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses. + +#include +#include +#include +#include +#include +#include +#include +#include +#if !defined(__FreeBSD__) && !defined(__APPLE__) +#include // for alloca +#endif +#include +#include "json_helpers.h" +#include "survive_config.h" +#include "survive_default_devices.h" + +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; +} + + +static int ParsePoints( SurviveContext * ctx, SurviveObject * so, char * ct0conf, FLT ** floats_out, jsmntok_t * t, int i ) +{ + int k; + int pts = t[i+1].size; + jsmntok_t * tk; + + so->nr_locations = 0; + *floats_out = malloc( sizeof( **floats_out ) * 32 * 3 ); + + for( k = 0; k < pts; k++ ) + { + tk = &t[i+2+k*4]; + + 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; + FLT f = atof( ctt ); + int id = so->nr_locations*3+m; + (*floats_out)[id] = f; + } + so->nr_locations++; + } + return 0; +} + +struct SurvivePlaybackData { + SurviveContext * ctx; + const char* playback_dir; + FILE* playback_file; + int lineno; + + uint64_t next_time_us; +}; +typedef struct SurvivePlaybackData SurvivePlaybackData; + + +uint64_t timestamp_in_us() { + static uint64_t start_time_us = 0; + struct timeval tv; + gettimeofday(&tv,NULL); + uint64_t now = (uint64_t)tv.tv_sec * 1000000L + tv.tv_usec; + if(start_time_us == 0) + start_time_us = now; + return now - start_time_us; +} + +static int parse_and_run_imu(const char* line, SurvivePlaybackData* driver) { + char dev[10]; + int timecode = 0; + FLT accelgyro[6]; + int mask; + int id; + + int rr = sscanf(line,"I %s %d %d " FLT_format " " FLT_format " " FLT_format " " FLT_format " " FLT_format " " FLT_format "%d", dev, + &mask, + &timecode, + &accelgyro[0], &accelgyro[1], &accelgyro[2], + &accelgyro[3], &accelgyro[4], &accelgyro[5], &id ); + + if( rr != 10 ) + { + fprintf( stderr, "Warning: On line %d, only %d values read: '%s'\n", driver->lineno, rr, line ); + return -1; + } + + SurviveObject * so = survive_get_so_by_name( driver->ctx, dev); + if(!so) { + fprintf(stderr, "Could not find device named %s from lineno %d\n", dev, driver->lineno); + return -1; + } + + driver->ctx->imuproc( so, mask, accelgyro, timecode, id); + return 0; +} + + +static int parse_and_run_lightcode(const char* line, SurvivePlaybackData* driver) { + char lhn[10]; + char axn[10]; + char dev[10]; + uint32_t timecode = 0; + int sensor = 0; + int acode = 0; + int timeinsweep = 0; + uint32_t pulselength = 0; + uint32_t lh = 0; + + int rr = sscanf(line,"%8s %8s %8s %u %d %d %d %u %u\n", + lhn, axn, dev, + &timecode, &sensor, &acode, + &timeinsweep, &pulselength, &lh ); + + if( rr != 9 ) + { + fprintf( stderr, "Warning: On line %d, only %d values read: '%s'\n", driver->lineno, rr, line ); + return -1; + } + + SurviveObject * so = survive_get_so_by_name( driver->ctx, dev); + if(!so) { + fprintf(stderr, "Could not find device named %s from lineno %d\n", dev, driver->lineno); + return -1; + } + + driver->ctx->lightproc( so, sensor, acode, timeinsweep, timecode, pulselength, lh); + return 0; +} + +static int playback_poll( struct SurviveContext * ctx, void * _driver ) { + SurvivePlaybackData* driver = _driver; + FILE* f = driver->playback_file; + + if(f && !feof(f) && !ferror(f) ) + { + int i; + driver->lineno++; + char * line; + + if(driver->next_time_us == 0) { + char * buffer; + size_t n = 0; + ssize_t r = getdelim( &line, &n, ' ', f ); + if( r <= 0 ) return 0; + + uint64_t timestamp; + if(sscanf(line, "%lu", &driver->next_time_us) != 1) { + free(line); + return 0; + } + free(line); + line = 0; + } + + if(driver->next_time_us > timestamp_in_us()) + return 0; + driver->next_time_us = 0; + + char * buffer; + size_t n = 0; + ssize_t r = getline( &line, &n, f ); + if( r <= 0 ) return 0; + + if((line[0] != 'R' && line[0] != 'L' && line[0] != 'I') || line[1] != ' ' ) + return 0; + + switch(line[0]) { + case 'L': + case 'R': + parse_and_run_lightcode(line, driver); + break; + case 'I': + parse_and_run_imu(line, driver); + break; + } + + free( line ); + } else { + if(f) { + fclose(driver->playback_file); + } + driver->playback_file = 0; + return -1; + } + + return 0; +} + +int playback_close( struct SurviveContext * ctx, void * _driver ) { + SurvivePlaybackData* driver = _driver; + if(driver->playback_file) + fclose(driver->playback_file); + driver->playback_file = 0; + return 0; +} + + +static int LoadConfig( SurvivePlaybackData * sv, SurviveObject * so, int devno, int iface, int extra_magic ) +{ + SurviveContext * ctx = sv->ctx; + char * ct0conf = 0; + + char fname[100]; + sprintf( fname, "%s/%s_config.json", sv->playback_dir, so->codename ); + FILE * f = fopen( fname, "r" ); + + if(f == 0 || feof(f) || ferror(f) ) + return 1; + + fseek(f, 0, SEEK_END); + int len = ftell(f); + fseek(f, 0, SEEK_SET); //same as rewind(f); + + ct0conf = malloc(len+1); + fread( ct0conf, len, 1, f); + fclose( f ); + ct0conf[len] = 0; + + printf( "Loading config: %d\n", len ); + + if (len == 0) + return 1; + + //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_INFO("Failed to parse JSON in HMD configuration: %d\n", r); + return -1; + } + if (r < 1 || t[0].type != JSMN_OBJECT) { + SV_INFO("Object expected in HMD configuration\n"); + return -2; + } + + 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, so, ct0conf, &so->sensor_locations, t, i ) ) + { + break; + } + } + if (jsoneq(ct0conf, tk, "modelNormals") == 0) { + if( ParsePoints( ctx, so, ct0conf, &so->sensor_normals, t, i ) ) + { + break; + } + } + + + if (jsoneq(ct0conf, tk, "acc_bias") == 0) { + int32_t count = (tk+1)->size; + FLT* values = NULL; + if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { + so->acc_bias = values; + so->acc_bias[0] *= .125; //XXX Wat? Observed by CNL. Biasing by more than this seems to hose things. + so->acc_bias[1] *= .125; + so->acc_bias[2] *= .125; + } + } + if (jsoneq(ct0conf, tk, "acc_scale") == 0) { + int32_t count = (tk+1)->size; + FLT* values = NULL; + if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { + so->acc_scale = values; + } + } + + if (jsoneq(ct0conf, tk, "gyro_bias") == 0) { + int32_t count = (tk+1)->size; + FLT* values = NULL; + if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { + so->gyro_bias = values; + } + } + if (jsoneq(ct0conf, tk, "gyro_scale") == 0) { + int32_t count = (tk+1)->size; + FLT* values = NULL; + if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { + so->gyro_scale = values; + } + } + } + + return 0; +} + + + +int DriverRegPlayback( SurviveContext * ctx ) +{ + const char* playback_dir = config_read_str(ctx->global_config_values, + "PlaybackDir", ""); + + if(strlen(playback_dir) == 0) { + return 0; +} + + SurvivePlaybackData * sp = calloc( 1, sizeof( SurvivePlaybackData ) ); + sp->ctx = ctx; + sp->playback_dir = playback_dir; + printf("%s\n", playback_dir); + + char playback_file[100]; + sprintf( playback_file, "%s/events", playback_dir ); + sp->playback_file = fopen( playback_file, "r"); + if(sp->playback_file == 0) { + fprintf(stderr, "Could not open playback events file %s", playback_file); + return -1; +} + SurviveObject * hmd = survive_create_hmd(ctx, "Playback", sp); + SurviveObject * wm0 = survive_create_wm0(ctx, "Playback", sp, 0); + SurviveObject * wm1 = survive_create_wm1(ctx, "Playback", sp, 0); + SurviveObject * tr0 = survive_create_tr0(ctx, "Playback", sp); + SurviveObject * ww0 = survive_create_ww0(ctx, "Playback", sp); + + if( !LoadConfig( sp, hmd, 1, 0, 0 )) { survive_add_object( ctx, hmd ); } + if( !LoadConfig( sp, wm0, 2, 0, 1 )) { survive_add_object( ctx, wm0 ); } + if( !LoadConfig( sp, wm1, 3, 0, 1 )) { survive_add_object( ctx, wm1 ); } + if( !LoadConfig( sp, tr0, 4, 0, 0 )) { survive_add_object( ctx, tr0 ); } + if( !LoadConfig( sp, ww0, 5, 0, 0 )) { survive_add_object( ctx, ww0 ); } + + + survive_add_driver(ctx, sp, playback_poll, playback_close, 0); + return 0; + fail_gracefully: + return -1; +} + +REGISTER_LINKTIME( DriverRegPlayback ); + -- cgit v1.2.3