Added playback device driver

1 files changed, 374 insertions, 0 deletions

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 <lubosz.sarnecki@collabora.co.uk>
+// 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 <survive.h>
+#include <jsmn.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <string.h>
+#include <sys/stat.h>
+#include <os_generic.h>
+#if !defined(__FreeBSD__) && !defined(__APPLE__)
+#include <malloc.h> // for alloca
+#endif
+#include <sys/time.h>
+#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 );
+