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 --- Makefile | 2 +- data_recorder.c | 118 +++++++----- include/libsurvive/survive_types.h | 6 + src/survive_default_devices.c | 46 +++++ src/survive_default_devices.h | 12 ++ src/survive_playback.c | 374 +++++++++++++++++++++++++++++++++++++ src/survive_vive.c | 101 +++------- 7 files changed, 539 insertions(+), 120 deletions(-) create mode 100644 src/survive_default_devices.c create mode 100644 src/survive_default_devices.h create mode 100755 src/survive_playback.c diff --git a/Makefile b/Makefile index 69b8e37..e57dae5 100644 --- a/Makefile +++ b/Makefile @@ -37,7 +37,7 @@ REDISTS:=redist/json_helpers.o redist/linmath.o redist/jsmn.o redist/os_generic. ifeq ($(UNAME), Darwin) REDISTS:=$(REDISTS) redist/hid-osx.c endif -LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_vive.o src/survive_config.o src/survive_cal.o +LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_default_devices.o src/survive_vive.o src/survive_playback.o src/survive_config.o src/survive_cal.o #If you want to use HIDAPI on Linux. diff --git a/data_recorder.c b/data_recorder.c index 002357e..323d208 100644 --- a/data_recorder.c +++ b/data_recorder.c @@ -3,6 +3,7 @@ #ifdef __linux__ #include #endif + #include #include #include @@ -10,9 +11,14 @@ #include #include #include +#include +#include +#include struct SurviveContext * ctx; +FILE* output_file = 0; + void HandleKey( int keycode, int bDown ) { if( !bDown ) return; @@ -43,56 +49,70 @@ int bufferpts[32*2*3]; char buffermts[32*128*3]; int buffertimeto[32*3]; +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; +} + +int write_to_output(const char *format, ...) +{ + va_list args; + va_start(args, format); + fprintf(output_file, "%lu ", timestamp_in_us()); + vfprintf(output_file, format, args); + + va_end(args); +} + void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh) { survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length, lh); - if( acode == -1 ) return; -//return; + if( acode == -1 ) { + write_to_output( "A %s %d %d %d %u %u %u\n", so->codename, sensor_id, acode, timeinsweep, timecode, length, lh ); + return; + } + int jumpoffset = sensor_id; if( strcmp( so->codename, "WM0" ) == 0 ) jumpoffset += 32; else if( strcmp( so->codename, "WM1" ) == 0 ) jumpoffset += 64; - - 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 ); - bufferpts[jumpoffset*2+0] = (timeinsweep-100000)/500; - buffertimeto[jumpoffset] = 0; - } - 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 ); - bufferpts[jumpoffset*2+1] = (timeinsweep-100000)/500; - buffertimeto[jumpoffset] = 0; - } - - - 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 ); - bufferpts[jumpoffset*2+0] = (timeinsweep-100000)/500; - buffertimeto[jumpoffset] = 0; - } - 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 ); - bufferpts[jumpoffset*2+1] = (timeinsweep-100000)/500; - buffertimeto[jumpoffset] = 0; - } + const char* LH_ID = 0; + const char* LH_Axis = 0; + + switch(acode) { + case 0: + case 2: + bufferpts[jumpoffset*2+0] = (timeinsweep-100000)/500; + LH_ID = "L"; LH_Axis = "X"; break; + case 1: + case 3: + bufferpts[jumpoffset*2+1] = (timeinsweep-100000)/500; + LH_ID = "L"; LH_Axis = "Y"; break; + case 4: + case 6: + bufferpts[jumpoffset*2+0] = (timeinsweep-100000)/500; + LH_ID = "R"; LH_Axis = "X"; break; + case 5: + case 7: + bufferpts[jumpoffset*2+1] = (timeinsweep-100000)/500; + LH_ID = "R"; LH_Axis = "Y"; break; + } + + write_to_output( "%s %s %s %u %d %d %d %u %u\n", LH_ID, LH_Axis, so->codename, timecode, sensor_id, acode, timeinsweep, length, lh ); + buffertimeto[jumpoffset] = 0; } void my_imu_process( struct SurviveObject * so, int mask, FLT * accelgyro, uint32_t timecode, int id ) { survive_default_imu_process( so, mask, accelgyro, timecode, id ); - -//return; - //if( so->codename[0] == 'H' ) - if( 1 ) - { - printf( "I %s %d %f %f %f %f %f %f %d\n", so->codename, timecode, accelgyro[0], accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id ); - } + write_to_output( "I %s %d %u %.17g %.17g %.17g %.17g %.17g %.17g %d\n", so->codename, mask, timecode, accelgyro[0], accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id ); } @@ -156,22 +176,30 @@ void *SurviveThread(void *junk) while(survive_poll(ctx) == 0) { - printf("Do stuff.\n"); - //Do stuff. } return 0; } -int main() +int main(int argc, char** argv) { - // Create the libsurvive thread - OGCreateThread(SurviveThread, 0); + if(argc > 1) { + output_file = fopen(argv[1], "w"); + if(output_file == 0) { + fprintf(stderr, "Could not open %s for writing", argv[1]); + return -1; + } + } else { + output_file = stdout; + } + + // Create the libsurvive thread + OGCreateThread(SurviveThread, 0); - // Wait for the survive thread to load - while (!SurviveThreadLoaded) { OGUSleep(100); } + // Wait for the survive thread to load + while (!SurviveThreadLoaded) { OGUSleep(100); } - // Run the Gui in the main thread - GuiThread(0); + // Run the Gui in the main thread + GuiThread(0); } diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h index 5384345..be1115b 100644 --- a/include/libsurvive/survive_types.h +++ b/include/libsurvive/survive_types.h @@ -14,6 +14,12 @@ extern "C" { #endif #endif +#define float_format "%f" +#define double_format "%lf" +#define _FLT_format2(f) f##_format +#define _FLT_format(f) _FLT_format2(f) +#define FLT_format _FLT_format(FLT) + typedef struct SurvivePose { FLT Pos[3]; diff --git a/src/survive_default_devices.c b/src/survive_default_devices.c new file mode 100644 index 0000000..5d2cda7 --- /dev/null +++ b/src/survive_default_devices.c @@ -0,0 +1,46 @@ +#include +#include +#include "survive_default_devices.h" + +static SurviveObject* survive_create_device(SurviveContext * ctx, + const char* driver_name, + void* driver, + const char* device_name, + haptic_func fn) { + SurviveObject * device = calloc( 1, sizeof( SurviveObject ) ); + + device->ctx = ctx; + device->driver = driver; + memcpy( device->codename, device_name, strlen(device_name) ); + memcpy( device->drivername, driver_name, strlen(driver_name) ); + + device->timebase_hz = 48000000; + device->pulsedist_max_ticks = 500000; + device->pulselength_min_sync = 2200; + device->pulse_in_clear_time = 35000; + device->pulse_max_for_sweep = 1800; + device->pulse_synctime_offset = 20000; + device->pulse_synctime_slack = 5000; + device->timecenter_ticks = device->timebase_hz / 240; + + device->haptic = fn; + + return device; +} + +SurviveObject* survive_create_hmd(SurviveContext * ctx, const char* driver_name, void* driver) { + return survive_create_device(ctx, driver_name, driver, "HMD", 0); +} + +SurviveObject* survive_create_wm0(SurviveContext * ctx, const char* driver_name, void* driver, haptic_func fn) { + return survive_create_device(ctx, driver_name, driver, "WM0", fn); +} +SurviveObject* survive_create_wm1(SurviveContext * ctx, const char* driver_name, void* driver, haptic_func fn) { + return survive_create_device(ctx, driver_name, driver, "WM1", fn); +} +SurviveObject* survive_create_tr0(SurviveContext * ctx, const char* driver_name, void* driver) { + return survive_create_device(ctx, driver_name, driver, "TR0", 0); +} +SurviveObject* survive_create_ww0(SurviveContext * ctx, const char* driver_name, void* driver) { + return survive_create_device(ctx, driver_name, driver, "WW0", 0); +} diff --git a/src/survive_default_devices.h b/src/survive_default_devices.h new file mode 100644 index 0000000..c2ebc0b --- /dev/null +++ b/src/survive_default_devices.h @@ -0,0 +1,12 @@ +#ifndef _SURVIVE_DEFAULT_DEVICES_H +#define _SURVIVE_DEFAULT_DEVICES_H + +#include + +SurviveObject* survive_create_hmd(SurviveContext * ctx, const char* driver_name, void* driver); +SurviveObject* survive_create_wm0(SurviveContext * ctx, const char* driver_name, void* driver, haptic_func cb); +SurviveObject* survive_create_wm1(SurviveContext * ctx, const char* driver_name, void* driver, haptic_func cb); +SurviveObject* survive_create_tr0(SurviveContext * ctx, const char* driver_name, void* driver); +SurviveObject* survive_create_ww0(SurviveContext * ctx, const char* driver_name, void* driver); + +#endif 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 ); + diff --git a/src/survive_vive.c b/src/survive_vive.c index cb05efc..e21d419 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -23,6 +23,7 @@ #endif #include "json_helpers.h" +#include "survive_default_devices.h" #ifdef HIDAPI #if defined(WINDOWS) || defined(WIN32) || defined (_WIN32) @@ -1699,15 +1700,16 @@ static int LoadConfig( SurviveViveData * sv, SurviveObject * so, int devno, int SurviveContext * ctx = sv->ctx; char * ct0conf = 0; int len = survive_get_config( &ct0conf, sv, devno, iface, extra_magic ); -printf( "Loading config: %d\n", len ); -#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 + printf( "Loading config: %d\n", len ); + { + char raw_fname[100]; + sprintf( raw_fname, "%s_config.json", so->codename ); + FILE * f = fopen( raw_fname, "w" ); + fwrite( ct0conf, strlen(ct0conf), 1, f ); + fclose( f ); + } + if( len > 0 ) { @@ -1836,18 +1838,13 @@ void init_SurviveObject(SurviveObject* so) { int DriverRegHTCVive( SurviveContext * ctx ) { int r; - SurviveObject * hmd = calloc( 1, sizeof( SurviveObject ) ); - SurviveObject * wm0 = calloc( 1, sizeof( SurviveObject ) ); - SurviveObject * wm1 = calloc( 1, sizeof( SurviveObject ) ); - SurviveObject * tr0 = calloc( 1, sizeof( SurviveObject ) ); - SurviveObject * ww0 = calloc( 1, sizeof( SurviveObject ) ); - SurviveViveData * sv = calloc( 1, sizeof( SurviveViveData ) ); - - init_SurviveObject(hmd); - init_SurviveObject(wm0); - init_SurviveObject(wm1); - init_SurviveObject(tr0); - init_SurviveObject(ww0); + + SurviveViveData * sv = calloc(1, sizeof(SurviveViveData) ); + SurviveObject * hmd = survive_create_hmd(ctx, "HTC", sv); + SurviveObject * wm0 = survive_create_wm0(ctx, "HTC", sv, 0); + SurviveObject * wm1 = survive_create_wm1(ctx, "HTC", sv, 0); + SurviveObject * tr0 = survive_create_tr0(ctx, "HTC", sv); + SurviveObject * ww0 = survive_create_ww0(ctx, "HTC", sv); sv->ctx = ctx; @@ -1859,28 +1856,6 @@ int DriverRegHTCVive( SurviveContext * ctx ) mkdir( "calinfo", 0755 ); #endif - - hmd->ctx = ctx; - hmd->driver = sv; - memcpy( hmd->codename, "HMD", 4 ); - memcpy( hmd->drivername, "HTC", 4 ); - wm0->ctx = ctx; - wm0->driver = sv; - memcpy( wm0->codename, "WM0", 4 ); - memcpy( wm0->drivername, "HTC", 4 ); - wm1->ctx = ctx; - wm1->driver = sv; - memcpy( wm1->codename, "WM1", 4 ); - memcpy( wm1->drivername, "HTC", 4 ); - tr0->ctx = ctx; - tr0->driver = sv; - memcpy( tr0->codename, "TR0", 4 ); - memcpy( tr0->drivername, "HTC", 4 ); - ww0->ctx = ctx; - ww0->driver = sv; - memcpy( ww0->codename, "WW0", 4 ); - memcpy( ww0->drivername, "HTC", 4 ); - //USB must happen last. if( r = survive_usb_init( sv, hmd, wm0, wm1, tr0, ww0) ) { @@ -1894,36 +1869,6 @@ int DriverRegHTCVive( SurviveContext * ctx ) if( sv->udev[USB_DEV_WATCHMAN2] && LoadConfig( sv, wm1, 3, 0, 1 )) { SV_INFO( "Watchman 1 config issue." ); } if( sv->udev[USB_DEV_TRACKER0] && LoadConfig( sv, tr0, 4, 0, 0 )) { SV_INFO( "Tracker 0 config issue." ); } if( sv->udev[USB_DEV_W_WATCHMAN1] && LoadConfig( sv, ww0, 5, 0, 0 )) { SV_INFO( "Wired Watchman 0 config issue." ); } - - hmd->timebase_hz = wm0->timebase_hz = wm1->timebase_hz = 48000000; - tr0->timebase_hz = ww0->timebase_hz = hmd->timebase_hz; - - hmd->pulsedist_max_ticks = wm0->pulsedist_max_ticks = wm1->pulsedist_max_ticks = 500000; - tr0->pulsedist_max_ticks = ww0->pulsedist_max_ticks = hmd->pulsedist_max_ticks; - - hmd->pulselength_min_sync = wm0->pulselength_min_sync = wm1->pulselength_min_sync = 2200; - tr0->pulselength_min_sync = ww0->pulselength_min_sync = hmd->pulselength_min_sync; - - hmd->pulse_in_clear_time = wm0->pulse_in_clear_time = wm1->pulse_in_clear_time = 35000; - tr0->pulse_in_clear_time = ww0->pulse_in_clear_time = hmd->pulse_in_clear_time; - - hmd->pulse_max_for_sweep = wm0->pulse_max_for_sweep = wm1->pulse_max_for_sweep = 1800; - tr0->pulse_max_for_sweep = ww0->pulse_max_for_sweep = hmd->pulse_max_for_sweep; - - hmd->pulse_synctime_offset = wm0->pulse_synctime_offset = wm1->pulse_synctime_offset = 20000; - tr0->pulse_synctime_offset = ww0->pulse_synctime_offset = hmd->pulse_synctime_offset; - - hmd->pulse_synctime_slack = wm0->pulse_synctime_slack = wm1->pulse_synctime_slack = 5000; - tr0->pulse_synctime_slack = ww0->pulse_synctime_slack = hmd->pulse_synctime_slack; - - hmd->timecenter_ticks = hmd->timebase_hz / 240; - wm0->timecenter_ticks = wm0->timebase_hz / 240; - wm1->timecenter_ticks = wm1->timebase_hz / 240; - tr0->timecenter_ticks = tr0->timebase_hz / 240; - ww0->timecenter_ticks = ww0->timebase_hz / 240; - - wm0->haptic = survive_vive_send_haptic; - wm1->haptic = survive_vive_send_haptic; /* int i; int locs = hmd->nr_locations; @@ -1945,7 +1890,7 @@ int DriverRegHTCVive( SurviveContext * ctx ) } } */ - + //Add the drivers. if( sv->udev[USB_DEV_HMD_IMU_LH] ) { survive_add_object( ctx, hmd ); } if( sv->udev[USB_DEV_WATCHMAN1] ) { survive_add_object( ctx, wm0 ); } @@ -1953,7 +1898,15 @@ int DriverRegHTCVive( SurviveContext * ctx ) if( sv->udev[USB_DEV_TRACKER0] ) { survive_add_object( ctx, tr0 ); } if( sv->udev[USB_DEV_W_WATCHMAN1] ) { survive_add_object( ctx, ww0 ); } - survive_add_driver( ctx, sv, survive_vive_usb_poll, survive_vive_close, survive_vive_send_magic ); + if( sv->udev[USB_DEV_HMD_IMU_LH] || + sv->udev[USB_DEV_WATCHMAN1] || + sv->udev[USB_DEV_WATCHMAN2] || + sv->udev[USB_DEV_TRACKER0] || + sv->udev[USB_DEV_W_WATCHMAN1] ) { + survive_add_driver( ctx, sv, survive_vive_usb_poll, survive_vive_close, survive_vive_send_magic ); + } else { + fprintf(stderr, "No USB devices detected\n"); + } return 0; fail_gracefully: -- cgit v1.2.3 From a90f380359b719befb96af624260057848e95dce Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Thu, 8 Mar 2018 15:30:19 -0700 Subject: Refactoring to reuse code standard to htc json format --- src/survive_default_devices.c | 154 +++++++++++++++++++++++++++++++++++++ src/survive_default_devices.h | 1 + src/survive_internal.h | 1 + src/survive_playback.c | 154 +++---------------------------------- src/survive_vive.c | 173 +----------------------------------------- 5 files changed, 168 insertions(+), 315 deletions(-) diff --git a/src/survive_default_devices.c b/src/survive_default_devices.c index 5d2cda7..1882002 100644 --- a/src/survive_default_devices.c +++ b/src/survive_default_devices.c @@ -1,6 +1,10 @@ +#include #include #include #include "survive_default_devices.h" +#include + +#include "json_helpers.h" static SurviveObject* survive_create_device(SurviveContext * ctx, const char* driver_name, @@ -44,3 +48,153 @@ SurviveObject* survive_create_tr0(SurviveContext * ctx, const char* driver_name, SurviveObject* survive_create_ww0(SurviveContext * ctx, const char* driver_name, void* driver) { return survive_create_device(ctx, driver_name, driver, "WW0", 0); } + + +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; +} + +int survive_load_htc_config_format(char* ct0conf, int len, SurviveObject * so) { + if (len == 0) + return -1; + + SurviveContext* ctx = so->ctx; + //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; + } + } + } + + + char fname[64]; + + sprintf( fname, "calinfo/%s_points.csv", so->codename ); + FILE * f = fopen( fname, "w" ); + int j; + for( j = 0; j < so->nr_locations; j++ ) + { + fprintf( f, "%f %f %f\n", so->sensor_locations[j*3+0], so->sensor_locations[j*3+1], so->sensor_locations[j*3+2] ); + } + fclose( f ); + + sprintf( fname, "calinfo/%s_normals.csv", so->codename ); + f = fopen( fname, "w" ); + for( j = 0; j < so->nr_locations; j++ ) + { + fprintf( f, "%f %f %f\n", so->sensor_normals[j*3+0], so->sensor_normals[j*3+1], so->sensor_normals[j*3+2] ); + } + fclose( f ); + + + return 0; +} diff --git a/src/survive_default_devices.h b/src/survive_default_devices.h index c2ebc0b..fce199d 100644 --- a/src/survive_default_devices.h +++ b/src/survive_default_devices.h @@ -9,4 +9,5 @@ SurviveObject* survive_create_wm1(SurviveContext * ctx, const char* driver_name, SurviveObject* survive_create_tr0(SurviveContext * ctx, const char* driver_name, void* driver); SurviveObject* survive_create_ww0(SurviveContext * ctx, const char* driver_name, void* driver); +int survive_load_htc_config_format(char* ct0conf, int length, SurviveObject * so); #endif diff --git a/src/survive_internal.h b/src/survive_internal.h index e1a733d..86b119f 100644 --- a/src/survive_internal.h +++ b/src/survive_internal.h @@ -17,6 +17,7 @@ void * GetDriver( const char * name ); const char * GetDriverNameMatching( const char * prefix, int place ); void ListDrivers(); + #endif diff --git a/src/survive_playback.c b/src/survive_playback.c index bb66f25..426a2b9 100755 --- a/src/survive_playback.c +++ b/src/survive_playback.c @@ -11,68 +11,15 @@ //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; @@ -215,7 +162,7 @@ static int playback_poll( struct SurviveContext * ctx, void * _driver ) { return 0; } -int playback_close( struct SurviveContext * ctx, void * _driver ) { +static int playback_close( struct SurviveContext * ctx, void * _driver ) { SurvivePlaybackData* driver = _driver; if(driver->playback_file) fclose(driver->playback_file); @@ -224,7 +171,7 @@ int playback_close( struct SurviveContext * ctx, void * _driver ) { } -static int LoadConfig( SurvivePlaybackData * sv, SurviveObject * so, int devno, int iface, int extra_magic ) +static int LoadConfig( SurvivePlaybackData * sv, SurviveObject * so) { SurviveContext * ctx = sv->ctx; char * ct0conf = 0; @@ -241,91 +188,12 @@ static int LoadConfig( SurvivePlaybackData * sv, SurviveObject * so, int devno, fseek(f, 0, SEEK_SET); //same as rewind(f); ct0conf = malloc(len+1); - fread( ct0conf, len, 1, f); + int read = 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; + return survive_load_htc_config_format(ct0conf, len, so); } @@ -357,11 +225,11 @@ int DriverRegPlayback( SurviveContext * ctx ) 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 ); } + if( !LoadConfig( sp, hmd )) { survive_add_object( ctx, hmd ); } + if( !LoadConfig( sp, wm0 )) { survive_add_object( ctx, wm0 ); } + if( !LoadConfig( sp, wm1 )) { survive_add_object( ctx, wm1 ); } + if( !LoadConfig( sp, tr0 )) { survive_add_object( ctx, tr0 ); } + if( !LoadConfig( sp, ww0 )) { survive_add_object( ctx, ww0 ); } survive_add_driver(ctx, sp, playback_poll, playback_close, 0); diff --git a/src/survive_vive.c b/src/survive_vive.c index e21d419..2472992 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -1648,53 +1648,6 @@ void survive_data_cb( SurviveUSBInterface * si ) -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; -} - static int LoadConfig( SurviveViveData * sv, SurviveObject * so, int devno, int iface, int extra_magic ) { SurviveContext * ctx = sv->ctx; @@ -1709,111 +1662,8 @@ static int LoadConfig( SurviveViveData * sv, SurviveObject * so, int devno, int fwrite( ct0conf, strlen(ct0conf), 1, f ); fclose( f ); } - - 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_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; - } - } - } - } - else - { - //TODO: Cleanup any remaining USB stuff. - return 1; - } - - char fname[64]; - - sprintf( fname, "calinfo/%s_points.csv", so->codename ); - FILE * f = fopen( fname, "w" ); - int j; - for( j = 0; j < so->nr_locations; j++ ) - { - fprintf( f, "%f %f %f\n", so->sensor_locations[j*3+0], so->sensor_locations[j*3+1], so->sensor_locations[j*3+2] ); - } - fclose( f ); - - sprintf( fname, "calinfo/%s_normals.csv", so->codename ); - f = fopen( fname, "w" ); - for( j = 0; j < so->nr_locations; j++ ) - { - fprintf( f, "%f %f %f\n", so->sensor_normals[j*3+0], so->sensor_normals[j*3+1], so->sensor_normals[j*3+2] ); - } - fclose( f ); - return 0; + return survive_load_htc_config_format(ct0conf, len, so); } @@ -1869,27 +1719,6 @@ int DriverRegHTCVive( SurviveContext * ctx ) if( sv->udev[USB_DEV_WATCHMAN2] && LoadConfig( sv, wm1, 3, 0, 1 )) { SV_INFO( "Watchman 1 config issue." ); } if( sv->udev[USB_DEV_TRACKER0] && LoadConfig( sv, tr0, 4, 0, 0 )) { SV_INFO( "Tracker 0 config issue." ); } if( sv->udev[USB_DEV_W_WATCHMAN1] && LoadConfig( sv, ww0, 5, 0, 0 )) { SV_INFO( "Wired Watchman 0 config issue." ); } -/* - int i; - int locs = hmd->nr_locations; - printf( "Locs: %d\n", locs ); - if (hmd->sensor_locations ) - { - printf( "POSITIONS:\n" ); - for( i = 0; i < locs*3; i+=3 ) - { - printf( "%f %f %f\n", hmd->sensor_locations[i+0], hmd->sensor_locations[i+1], hmd->sensor_locations[i+2] ); - } - } - if( hmd->sensor_normals ) - { - printf( "NORMALS:\n" ); - for( i = 0; i < locs*3; i+=3 ) - { - printf( "%f %f %f\n", hmd->sensor_normals[i+0], hmd->sensor_normals[i+1], hmd->sensor_normals[i+2] ); - } - } -*/ //Add the drivers. if( sv->udev[USB_DEV_HMD_IMU_LH] ) { survive_add_object( ctx, hmd ); } -- cgit v1.2.3 From 58de3587d9e4d620daa57268cc74092120b7caac Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Thu, 8 Mar 2018 16:13:40 -0700 Subject: Disable usb driver if playback is active --- src/survive_vive.c | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/survive_vive.c b/src/survive_vive.c index 2472992..288f8cb 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -24,6 +24,7 @@ #include "json_helpers.h" #include "survive_default_devices.h" +#include "survive_config.h" #ifdef HIDAPI #if defined(WINDOWS) || defined(WIN32) || defined (_WIN32) @@ -1687,8 +1688,14 @@ void init_SurviveObject(SurviveObject* so) { int DriverRegHTCVive( SurviveContext * ctx ) { - int r; - + const char* playback_dir = config_read_str(ctx->global_config_values, + "PlaybackDir", ""); + if(strlen(playback_dir) != 0) { + SV_INFO("Playback is active; disabling USB driver"); + return 0; + } + + int r; SurviveViveData * sv = calloc(1, sizeof(SurviveViveData) ); SurviveObject * hmd = survive_create_hmd(ctx, "HTC", sv); SurviveObject * wm0 = survive_create_wm0(ctx, "HTC", sv, 0); -- cgit v1.2.3 From 82dc5c4452e2a58667c337e967200cb7760c16cd Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Thu, 8 Mar 2018 16:19:05 -0700 Subject: Added time factor to playback --- src/survive_playback.c | 19 ++++++------------- 1 file changed, 6 insertions(+), 13 deletions(-) diff --git a/src/survive_playback.c b/src/survive_playback.c index 426a2b9..d8c6fba 100755 --- a/src/survive_playback.c +++ b/src/survive_playback.c @@ -1,13 +1,3 @@ -//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 @@ -26,6 +16,7 @@ struct SurvivePlaybackData { FILE* playback_file; int lineno; + FLT time_factor; uint64_t next_time_us; }; typedef struct SurvivePlaybackData SurvivePlaybackData; @@ -128,7 +119,7 @@ static int playback_poll( struct SurviveContext * ctx, void * _driver ) { line = 0; } - if(driver->next_time_us > timestamp_in_us()) + if(driver->next_time_us * driver->time_factor > timestamp_in_us()) return 0; driver->next_time_us = 0; @@ -204,12 +195,14 @@ int DriverRegPlayback( SurviveContext * ctx ) "PlaybackDir", ""); if(strlen(playback_dir) == 0) { - return 0; -} + return 0; + } SurvivePlaybackData * sp = calloc( 1, sizeof( SurvivePlaybackData ) ); sp->ctx = ctx; sp->playback_dir = playback_dir; + sp->time_factor = config_read_float(ctx->global_config_values, "PlaybackFactor", 1.); + printf("%s\n", playback_dir); char playback_file[100]; -- cgit v1.2.3 From 3ec96c9a81f0f770fac75a1bdfc16ea96df07d9b Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Fri, 9 Mar 2018 00:21:17 -0700 Subject: Swapped to OGGetAbsoluteTime for timestamp --- data_recorder.c | 15 +++++++-------- src/survive_playback.c | 20 +++++++++----------- 2 files changed, 16 insertions(+), 19 deletions(-) diff --git a/data_recorder.c b/data_recorder.c index 323d208..63746c3 100644 --- a/data_recorder.c +++ b/data_recorder.c @@ -15,6 +15,8 @@ #include #include +#include "redist/os_generic.h" + struct SurviveContext * ctx; FILE* output_file = 0; @@ -49,21 +51,18 @@ int bufferpts[32*2*3]; char buffermts[32*128*3]; int buffertimeto[32*3]; -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; +double timestamp_in_us() { + static double start_time_us = 0; if(start_time_us == 0) - start_time_us = now; - return now - start_time_us; + start_time_us = OGGetAbsoluteTime(); + return OGGetAbsoluteTime() - start_time_us; } int write_to_output(const char *format, ...) { va_list args; va_start(args, format); - fprintf(output_file, "%lu ", timestamp_in_us()); + fprintf(output_file, "%.17g ", timestamp_in_us()); vfprintf(output_file, format, args); va_end(args); diff --git a/src/survive_playback.c b/src/survive_playback.c index d8c6fba..7c49f5b 100755 --- a/src/survive_playback.c +++ b/src/survive_playback.c @@ -10,6 +10,8 @@ #include "survive_config.h" #include "survive_default_devices.h" +#include "redist/os_generic.h" + struct SurvivePlaybackData { SurviveContext * ctx; const char* playback_dir; @@ -17,19 +19,16 @@ struct SurvivePlaybackData { int lineno; FLT time_factor; - uint64_t next_time_us; + double 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; +double timestamp_in_us() { + static double start_time_us = 0; + if(start_time_us == 0.) + start_time_us = OGGetAbsoluteTime(); + return OGGetAbsoluteTime() - start_time_us; } static int parse_and_run_imu(const char* line, SurvivePlaybackData* driver) { @@ -110,8 +109,7 @@ static int playback_poll( struct SurviveContext * ctx, void * _driver ) { ssize_t r = getdelim( &line, &n, ' ', f ); if( r <= 0 ) return 0; - uint64_t timestamp; - if(sscanf(line, "%lu", &driver->next_time_us) != 1) { + if(sscanf(line, "%lf", &driver->next_time_us) != 1) { free(line); return 0; } -- cgit v1.2.3 From fe88ef57cff4971664b252cb9def6a5ddcc4276d Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Fri, 9 Mar 2018 00:41:46 -0700 Subject: Spaces -> Tabs (code formatting change) --- data_recorder.c | 250 ++++++++++++++-------------- src/survive_default_devices.c | 309 +++++++++++++++++----------------- src/survive_default_devices.h | 18 +- src/survive_playback.c | 375 +++++++++++++++++++++--------------------- 4 files changed, 483 insertions(+), 469 deletions(-) mode change 100755 => 100644 src/survive_playback.c diff --git a/data_recorder.c b/data_recorder.c index 63746c3..6123d57 100644 --- a/data_recorder.c +++ b/data_recorder.c @@ -1,204 +1,204 @@ -//Data recorder mod with GUI showing light positions. +// Data recorder mod with GUI showing light positions. #ifdef __linux__ #include #endif +#include +#include +#include +#include #include #include -#include -#include #include -#include -#include -#include +#include #include -#include +#include #include "redist/os_generic.h" -struct SurviveContext * ctx; +struct SurviveContext *ctx; -FILE* output_file = 0; +FILE *output_file = 0; -void HandleKey( int keycode, int bDown ) -{ - if( !bDown ) return; +void HandleKey(int keycode, int bDown) { + if (!bDown) + return; - if( keycode == 'O' || keycode == 'o' ) - { - survive_send_magic(ctx,1,0,0); + if (keycode == 'O' || keycode == 'o') { + survive_send_magic(ctx, 1, 0, 0); } - if( keycode == 'F' || keycode == 'f' ) - { - survive_send_magic(ctx,0,0,0); + if (keycode == 'F' || keycode == 'f') { + survive_send_magic(ctx, 0, 0, 0); } } -void HandleButton( int x, int y, int button, int bDown ) -{ -} +void HandleButton(int x, int y, int button, int bDown) {} -void HandleMotion( int x, int y, int mask ) -{ -} +void HandleMotion(int x, int y, int mask) {} -void HandleDestroy() -{ -} +void HandleDestroy() {} -int bufferpts[32*2*3]; -char buffermts[32*128*3]; -int buffertimeto[32*3]; +int bufferpts[32 * 2 * 3]; +char buffermts[32 * 128 * 3]; +int buffertimeto[32 * 3]; double timestamp_in_us() { - static double start_time_us = 0; - if(start_time_us == 0) - start_time_us = OGGetAbsoluteTime(); - return OGGetAbsoluteTime() - start_time_us; + static double start_time_us = 0; + if (start_time_us == 0) + start_time_us = OGGetAbsoluteTime(); + return OGGetAbsoluteTime() - start_time_us; } -int write_to_output(const char *format, ...) -{ - va_list args; - va_start(args, format); - fprintf(output_file, "%.17g ", timestamp_in_us()); - vfprintf(output_file, format, args); +int write_to_output(const char *format, ...) { + va_list args; + va_start(args, format); + fprintf(output_file, "%.17g ", timestamp_in_us()); + vfprintf(output_file, format, args); - va_end(args); + va_end(args); } -void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh) -{ - survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length, lh); +void my_light_process(struct SurviveObject *so, int sensor_id, int acode, + int timeinsweep, uint32_t timecode, uint32_t length, + uint32_t lh) { + survive_default_light_process(so, sensor_id, acode, timeinsweep, timecode, + length, lh); - if( acode == -1 ) { - write_to_output( "A %s %d %d %d %u %u %u\n", so->codename, sensor_id, acode, timeinsweep, timecode, length, lh ); - return; + if (acode == -1) { + write_to_output("A %s %d %d %d %u %u %u\n", so->codename, sensor_id, + acode, timeinsweep, timecode, length, lh); + return; } int jumpoffset = sensor_id; - if( strcmp( so->codename, "WM0" ) == 0 ) jumpoffset += 32; - else if( strcmp( so->codename, "WM1" ) == 0 ) jumpoffset += 64; + if (strcmp(so->codename, "WM0") == 0) + jumpoffset += 32; + else if (strcmp(so->codename, "WM1") == 0) + jumpoffset += 64; + + const char *LH_ID = 0; + const char *LH_Axis = 0; - const char* LH_ID = 0; - const char* LH_Axis = 0; - - switch(acode) { + switch (acode) { case 0: case 2: - bufferpts[jumpoffset*2+0] = (timeinsweep-100000)/500; - LH_ID = "L"; LH_Axis = "X"; break; + bufferpts[jumpoffset * 2 + 0] = (timeinsweep - 100000) / 500; + LH_ID = "L"; + LH_Axis = "X"; + break; case 1: case 3: - bufferpts[jumpoffset*2+1] = (timeinsweep-100000)/500; - LH_ID = "L"; LH_Axis = "Y"; break; + bufferpts[jumpoffset * 2 + 1] = (timeinsweep - 100000) / 500; + LH_ID = "L"; + LH_Axis = "Y"; + break; case 4: case 6: - bufferpts[jumpoffset*2+0] = (timeinsweep-100000)/500; - LH_ID = "R"; LH_Axis = "X"; break; + bufferpts[jumpoffset * 2 + 0] = (timeinsweep - 100000) / 500; + LH_ID = "R"; + LH_Axis = "X"; + break; case 5: case 7: - bufferpts[jumpoffset*2+1] = (timeinsweep-100000)/500; - LH_ID = "R"; LH_Axis = "Y"; break; - } + bufferpts[jumpoffset * 2 + 1] = (timeinsweep - 100000) / 500; + LH_ID = "R"; + LH_Axis = "Y"; + break; + } - write_to_output( "%s %s %s %u %d %d %d %u %u\n", LH_ID, LH_Axis, so->codename, timecode, sensor_id, acode, timeinsweep, length, lh ); + write_to_output("%s %s %s %u %d %d %d %u %u\n", LH_ID, LH_Axis, + so->codename, timecode, sensor_id, acode, timeinsweep, + length, lh); buffertimeto[jumpoffset] = 0; - } -void my_imu_process( struct SurviveObject * so, int mask, FLT * accelgyro, uint32_t timecode, int id ) -{ - survive_default_imu_process( so, mask, accelgyro, timecode, id ); - write_to_output( "I %s %d %u %.17g %.17g %.17g %.17g %.17g %.17g %d\n", so->codename, mask, timecode, accelgyro[0], accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id ); +void my_imu_process(struct SurviveObject *so, int mask, FLT *accelgyro, + uint32_t timecode, int id) { + survive_default_imu_process(so, mask, accelgyro, timecode, id); + write_to_output("I %s %d %u %.17g %.17g %.17g %.17g %.17g %.17g %d\n", + so->codename, mask, timecode, accelgyro[0], accelgyro[1], + accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id); } - - - -void * GuiThread( void * v ) -{ +void *GuiThread(void *v) { CNFGBGColor = 0x000000; CNFGDialogColor = 0x444444; - CNFGSetup( "Survive GUI Debug", 640, 480 ); + CNFGSetup("Survive GUI Debug", 640, 480); short screenx, screeny; - while(1) - { + while (1) { CNFGHandleInput(); CNFGClearFrame(); - CNFGColor( 0xFFFFFF ); - CNFGGetDimensions( &screenx, &screeny ); + CNFGColor(0xFFFFFF); + CNFGGetDimensions(&screenx, &screeny); int i; - for( i = 0; i < 32*3; i++ ) - { - if( buffertimeto[i] < 50 ) - { + for (i = 0; i < 32 * 3; i++) { + if (buffertimeto[i] < 50) { 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 ); + 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 ); + OGUSleep(10000); } } -int SurviveThreadLoaded=0; +int SurviveThreadLoaded = 0; -void *SurviveThread(void *junk) -{ - ctx = survive_init( 0 ); +void *SurviveThread(void *junk) { + ctx = survive_init(0); - survive_install_light_fn( ctx, my_light_process ); - survive_install_imu_fn( ctx, my_imu_process ); + survive_install_light_fn(ctx, my_light_process); + survive_install_imu_fn(ctx, my_imu_process); - if( !ctx ) - { - fprintf( stderr, "Fatal. Could not start\n" ); + if (!ctx) { + fprintf(stderr, "Fatal. Could not start\n"); exit(1); } SurviveThreadLoaded = 1; - while(survive_poll(ctx) == 0) - { + while (survive_poll(ctx) == 0) { } - return 0; + return 0; } -int main(int argc, char** argv) -{ - if(argc > 1) { - output_file = fopen(argv[1], "w"); - if(output_file == 0) { - fprintf(stderr, "Could not open %s for writing", argv[1]); - return -1; - } - } else { - output_file = stdout; - } - - // Create the libsurvive thread - OGCreateThread(SurviveThread, 0); - - // Wait for the survive thread to load - while (!SurviveThreadLoaded) { OGUSleep(100); } - - // Run the Gui in the main thread - GuiThread(0); -} +int main(int argc, char **argv) { + if (argc > 1) { + output_file = fopen(argv[1], "w"); + if (output_file == 0) { + fprintf(stderr, "Could not open %s for writing", argv[1]); + return -1; + } + } else { + output_file = stdout; + } + + // Create the libsurvive thread + OGCreateThread(SurviveThread, 0); + // Wait for the survive thread to load + while (!SurviveThreadLoaded) { + OGUSleep(100); + } + + // Run the Gui in the main thread + GuiThread(0); +} diff --git a/src/survive_default_devices.c b/src/survive_default_devices.c index 1882002..6615f1e 100644 --- a/src/survive_default_devices.c +++ b/src/survive_default_devices.c @@ -1,93 +1,92 @@ +#include "survive_default_devices.h" #include +#include #include #include -#include "survive_default_devices.h" -#include #include "json_helpers.h" -static SurviveObject* survive_create_device(SurviveContext * ctx, - const char* driver_name, - void* driver, - const char* device_name, - haptic_func fn) { - SurviveObject * device = calloc( 1, sizeof( SurviveObject ) ); - - device->ctx = ctx; - device->driver = driver; - memcpy( device->codename, device_name, strlen(device_name) ); - memcpy( device->drivername, driver_name, strlen(driver_name) ); - - device->timebase_hz = 48000000; - device->pulsedist_max_ticks = 500000; - device->pulselength_min_sync = 2200; - device->pulse_in_clear_time = 35000; - device->pulse_max_for_sweep = 1800; - device->pulse_synctime_offset = 20000; - device->pulse_synctime_slack = 5000; - device->timecenter_ticks = device->timebase_hz / 240; - - device->haptic = fn; - - return device; +static SurviveObject * +survive_create_device(SurviveContext *ctx, const char *driver_name, + void *driver, const char *device_name, haptic_func fn) { + SurviveObject *device = calloc(1, sizeof(SurviveObject)); + + device->ctx = ctx; + device->driver = driver; + memcpy(device->codename, device_name, strlen(device_name)); + memcpy(device->drivername, driver_name, strlen(driver_name)); + + device->timebase_hz = 48000000; + device->pulsedist_max_ticks = 500000; + device->pulselength_min_sync = 2200; + device->pulse_in_clear_time = 35000; + device->pulse_max_for_sweep = 1800; + device->pulse_synctime_offset = 20000; + device->pulse_synctime_slack = 5000; + device->timecenter_ticks = device->timebase_hz / 240; + + device->haptic = fn; + + return device; } -SurviveObject* survive_create_hmd(SurviveContext * ctx, const char* driver_name, void* driver) { - return survive_create_device(ctx, driver_name, driver, "HMD", 0); +SurviveObject *survive_create_hmd(SurviveContext *ctx, const char *driver_name, + void *driver) { + return survive_create_device(ctx, driver_name, driver, "HMD", 0); } -SurviveObject* survive_create_wm0(SurviveContext * ctx, const char* driver_name, void* driver, haptic_func fn) { - return survive_create_device(ctx, driver_name, driver, "WM0", fn); +SurviveObject *survive_create_wm0(SurviveContext *ctx, const char *driver_name, + void *driver, haptic_func fn) { + return survive_create_device(ctx, driver_name, driver, "WM0", fn); } -SurviveObject* survive_create_wm1(SurviveContext * ctx, const char* driver_name, void* driver, haptic_func fn) { - return survive_create_device(ctx, driver_name, driver, "WM1", fn); +SurviveObject *survive_create_wm1(SurviveContext *ctx, const char *driver_name, + void *driver, haptic_func fn) { + return survive_create_device(ctx, driver_name, driver, "WM1", fn); } -SurviveObject* survive_create_tr0(SurviveContext * ctx, const char* driver_name, void* driver) { - return survive_create_device(ctx, driver_name, driver, "TR0", 0); +SurviveObject *survive_create_tr0(SurviveContext *ctx, const char *driver_name, + void *driver) { + return survive_create_device(ctx, driver_name, driver, "TR0", 0); } -SurviveObject* survive_create_ww0(SurviveContext * ctx, const char* driver_name, void* driver) { - return survive_create_device(ctx, driver_name, driver, "WW0", 0); +SurviveObject *survive_create_ww0(SurviveContext *ctx, const char *driver_name, + void *driver) { + return survive_create_device(ctx, driver_name, driver, "WW0", 0); } - 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) { + 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 ) -{ +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; + int pts = t[i + 1].size; + jsmntok_t *tk; so->nr_locations = 0; - *floats_out = malloc( sizeof( **floats_out ) * 32 * 3 ); + *floats_out = malloc(sizeof(**floats_out) * 32 * 3); - for( k = 0; k < pts; k++ ) - { - tk = &t[i+2+k*4]; + for (k = 0; k < pts; k++) { + tk = &t[i + 2 + k * 4]; int m; - for( m = 0; m < 3; 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" ); + if (tk->type != 4 || elemlen > sizeof(ctt) - 1) { + SV_ERROR("Parse error in JSON\n"); return 1; } - memcpy( ctt, ct0conf + tk->start, elemlen ); + memcpy(ctt, ct0conf + tk->start, elemlen); ctt[elemlen] = 0; - FLT f = atof( ctt ); - int id = so->nr_locations*3+m; + FLT f = atof(ctt); + int id = so->nr_locations * 3 + m; (*floats_out)[id] = f; } so->nr_locations++; @@ -95,106 +94,106 @@ static int ParsePoints( SurviveContext * ctx, SurviveObject * so, char * ct0conf return 0; } -int survive_load_htc_config_format(char* ct0conf, int len, SurviveObject * so) { - if (len == 0) - return -1; - - SurviveContext* ctx = so->ctx; - //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; +int survive_load_htc_config_format(char *ct0conf, int len, SurviveObject *so) { + if (len == 0) + return -1; + + SurviveContext *ctx = so->ctx; + // 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 (jsoneq(ct0conf, tk, "modelNormals") == 0) { - if( ParsePoints( ctx, so, ct0conf, &so->sensor_normals, t, i ) ) - { - break; + if (r < 1 || t[0].type != JSMN_OBJECT) { + SV_INFO("Object expected in HMD configuration\n"); + return -2; } - } - - - 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; - } - } - } - - - char fname[64]; - - sprintf( fname, "calinfo/%s_points.csv", so->codename ); - FILE * f = fopen( fname, "w" ); - int j; - for( j = 0; j < so->nr_locations; j++ ) - { - fprintf( f, "%f %f %f\n", so->sensor_locations[j*3+0], so->sensor_locations[j*3+1], so->sensor_locations[j*3+2] ); - } - fclose( f ); - - sprintf( fname, "calinfo/%s_normals.csv", so->codename ); - f = fopen( fname, "w" ); - for( j = 0; j < so->nr_locations; j++ ) - { - fprintf( f, "%f %f %f\n", so->sensor_normals[j*3+0], so->sensor_normals[j*3+1], so->sensor_normals[j*3+2] ); - } - fclose( f ); - - - 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, 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; + } + } + } + + char fname[64]; + + sprintf(fname, "calinfo/%s_points.csv", so->codename); + FILE *f = fopen(fname, "w"); + int j; + for (j = 0; j < so->nr_locations; j++) { + fprintf(f, "%f %f %f\n", so->sensor_locations[j * 3 + 0], + so->sensor_locations[j * 3 + 1], + so->sensor_locations[j * 3 + 2]); + } + fclose(f); + + sprintf(fname, "calinfo/%s_normals.csv", so->codename); + f = fopen(fname, "w"); + for (j = 0; j < so->nr_locations; j++) { + fprintf(f, "%f %f %f\n", so->sensor_normals[j * 3 + 0], + so->sensor_normals[j * 3 + 1], so->sensor_normals[j * 3 + 2]); + } + fclose(f); + + return 0; } diff --git a/src/survive_default_devices.h b/src/survive_default_devices.h index fce199d..1fcca72 100644 --- a/src/survive_default_devices.h +++ b/src/survive_default_devices.h @@ -3,11 +3,17 @@ #include -SurviveObject* survive_create_hmd(SurviveContext * ctx, const char* driver_name, void* driver); -SurviveObject* survive_create_wm0(SurviveContext * ctx, const char* driver_name, void* driver, haptic_func cb); -SurviveObject* survive_create_wm1(SurviveContext * ctx, const char* driver_name, void* driver, haptic_func cb); -SurviveObject* survive_create_tr0(SurviveContext * ctx, const char* driver_name, void* driver); -SurviveObject* survive_create_ww0(SurviveContext * ctx, const char* driver_name, void* driver); +SurviveObject *survive_create_hmd(SurviveContext *ctx, const char *driver_name, + void *driver); +SurviveObject *survive_create_wm0(SurviveContext *ctx, const char *driver_name, + void *driver, haptic_func cb); +SurviveObject *survive_create_wm1(SurviveContext *ctx, const char *driver_name, + void *driver, haptic_func cb); +SurviveObject *survive_create_tr0(SurviveContext *ctx, const char *driver_name, + void *driver); +SurviveObject *survive_create_ww0(SurviveContext *ctx, const char *driver_name, + void *driver); -int survive_load_htc_config_format(char* ct0conf, int length, SurviveObject * so); +int survive_load_htc_config_format(char *ct0conf, int length, + SurviveObject *so); #endif diff --git a/src/survive_playback.c b/src/survive_playback.c old mode 100755 new mode 100644 index 7c49f5b..4b25b4c --- a/src/survive_playback.c +++ b/src/survive_playback.c @@ -1,8 +1,9 @@ -//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses. +// All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL +// or LGPL licenses. -#include #include #include +#include #include #include @@ -13,221 +14,229 @@ #include "redist/os_generic.h" struct SurvivePlaybackData { - SurviveContext * ctx; - const char* playback_dir; - FILE* playback_file; - int lineno; + SurviveContext *ctx; + const char *playback_dir; + FILE *playback_file; + int lineno; - FLT time_factor; - double next_time_us; + FLT time_factor; + double next_time_us; }; typedef struct SurvivePlaybackData SurvivePlaybackData; - double timestamp_in_us() { - static double start_time_us = 0; - if(start_time_us == 0.) - start_time_us = OGGetAbsoluteTime(); - return OGGetAbsoluteTime() - start_time_us; + static double start_time_us = 0; + if (start_time_us == 0.) + start_time_us = OGGetAbsoluteTime(); + return OGGetAbsoluteTime() - 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_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; + } -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; + 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); + + 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; - - if(sscanf(line, "%lf", &driver->next_time_us) != 1) { - free(line); - 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; + + if (sscanf(line, "%lf", &driver->next_time_us) != 1) { + free(line); + return 0; + } + free(line); + line = 0; + } + + if (driver->next_time_us * driver->time_factor > 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; } - free(line); - line = 0; - } - if(driver->next_time_us * driver->time_factor > 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; } -static 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 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) { + SurviveContext *ctx = sv->ctx; + char *ct0conf = 0; -static int LoadConfig( SurvivePlaybackData * sv, SurviveObject * so) -{ - 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" ); + 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; - 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); + fseek(f, 0, SEEK_SET); // same as rewind(f); - ct0conf = malloc(len+1); - int read = fread( ct0conf, len, 1, f); - fclose( f ); + ct0conf = malloc(len + 1); + int read = fread(ct0conf, len, 1, f); + fclose(f); ct0conf[len] = 0; - printf( "Loading config: %d\n", len ); + printf("Loading config: %d\n", len); return survive_load_htc_config_format(ct0conf, len, so); } +int DriverRegPlayback(SurviveContext *ctx) { + const char *playback_dir = + config_read_str(ctx->global_config_values, "PlaybackDir", ""); + if (strlen(playback_dir) == 0) { + 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; - sp->time_factor = config_read_float(ctx->global_config_values, "PlaybackFactor", 1.); - - printf("%s\n", playback_dir); + SurvivePlaybackData *sp = calloc(1, sizeof(SurvivePlaybackData)); + sp->ctx = ctx; + sp->playback_dir = playback_dir; + sp->time_factor = + config_read_float(ctx->global_config_values, "PlaybackFactor", 1.); + + 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)) { + survive_add_object(ctx, hmd); + } + if (!LoadConfig(sp, wm0)) { + survive_add_object(ctx, wm0); + } + if (!LoadConfig(sp, wm1)) { + survive_add_object(ctx, wm1); + } + if (!LoadConfig(sp, tr0)) { + survive_add_object(ctx, tr0); + } + if (!LoadConfig(sp, ww0)) { + survive_add_object(ctx, ww0); + } - 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 )) { survive_add_object( ctx, hmd ); } - if( !LoadConfig( sp, wm0 )) { survive_add_object( ctx, wm0 ); } - if( !LoadConfig( sp, wm1 )) { survive_add_object( ctx, wm1 ); } - if( !LoadConfig( sp, tr0 )) { survive_add_object( ctx, tr0 ); } - if( !LoadConfig( sp, ww0 )) { survive_add_object( ctx, ww0 ); } - - - survive_add_driver(ctx, sp, playback_poll, playback_close, 0); - return 0; - fail_gracefully: - return -1; + survive_add_driver(ctx, sp, playback_poll, playback_close, 0); + return 0; +fail_gracefully: + return -1; } -REGISTER_LINKTIME( DriverRegPlayback ); - +REGISTER_LINKTIME(DriverRegPlayback); -- cgit v1.2.3