Update code to allow for headless mode.

8 files changed, 252 insertions, 23 deletions

diff --git a/Makefile b/Makefile
index fb74e75..9940260 100644
--- a/Makefile
+++ b/Makefile
@@ -1,4 +1,4 @@
-all : lib data_recorder test calibrate
+all : lib data_recorder test calibrate calibrate_client
 
 CFLAGS:=-Iinclude -fPIC -g -O0 -Iredist -flto -DUSE_DOUBLE
 LDFLAGS:=-lpthread -lusb-1.0 -lz -lX11 -lm -flto -g
@@ -17,6 +17,9 @@ data_recorder : data_recorder.c lib/libsurvive.so redist/os_generic.o redist/Dra
 calibrate :  calibrate.c lib/libsurvive.so redist/os_generic.c redist/DrawFunctions.c redist/XDriver.c
 	gcc -o $@ $^ $(LDFLAGS) $(CFLAGS)
 
+calibrate_client :  calibrate_client.c lib/libsurvive.so redist/os_generic.c redist/DrawFunctions.c redist/XDriver.c
+	gcc -o $@ $^ $(LDFLAGS) $(CFLAGS)
+
 lib:
 	mkdir lib
 
diff --git a/calibrate.c b/calibrate.c
index 04ea9a8..8eedc88 100644
--- a/calibrate.c
+++ b/calibrate.c
@@ -140,7 +140,7 @@ void * GuiThread( void * v )
 
 int main()
 {
-	ctx = survive_init( );
+	ctx = survive_init( 0 );
 
 	survive_install_light_fn( ctx,  my_light_process );
 	survive_install_imu_fn( ctx,  my_imu_process );
diff --git a/calibrate_client.c b/calibrate_client.c
new file mode 100644
index 0000000..25b6303
--- /dev/null
+++ b/calibrate_client.c
@@ -0,0 +1,212 @@
+//Totally hacky "client" for absorbing data from a network source of the vive data.
+
+#include <unistd.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <survive.h>
+#include <string.h>
+#include <os_generic.h>
+#include "src/survive_cal.h"
+#include <DrawFunctions.h>
+
+struct SurviveContext * ctx;
+
+void HandleKey( int keycode, int bDown )
+{
+	if( !bDown ) return;
+
+	if( keycode == 'O' || keycode == 'o' )
+	{
+		survive_usb_send_magic(ctx,1);
+	}
+	if( keycode == 'F' || keycode == 'f' )
+	{
+		survive_usb_send_magic(ctx,0);
+	}
+}
+
+void HandleButton( int x, int y, int button, int bDown )
+{
+}
+
+void HandleMotion( int x, int y, int mask )
+{
+}
+
+int bufferpts[32*2*3];
+char buffermts[32*128*3];
+int buffertimeto[32*3];
+
+void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length  )
+{
+	survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length );
+
+	if( acode == -1 ) return;
+//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
+	{
+		bufferpts[jumpoffset*2+0] = (timeinsweep-100000)/500;
+		buffertimeto[jumpoffset] = 0;
+	}
+	if( acode == 1 || acode == 3 ) //data = 1
+	{
+		bufferpts[jumpoffset*2+1] = (timeinsweep-100000)/500;
+		buffertimeto[jumpoffset] = 0;
+	}
+
+
+	if( acode == 4 || acode == 6 ) //data = 0
+	{
+		bufferpts[jumpoffset*2+0] = (timeinsweep-100000)/500;
+		buffertimeto[jumpoffset] = 0;
+	}
+	if( acode == 5 || acode == 7 ) //data = 1
+	{
+		bufferpts[jumpoffset*2+1] = (timeinsweep-100000)/500;
+		buffertimeto[jumpoffset] = 0;
+	}
+}
+
+void my_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id )
+{
+	survive_default_imu_process( so, accelgyro, timecode, id );
+
+return;
+	//if( so->codename[0] == 'H' )
+	if( 1 )
+	{
+		printf( "I %s %d %d %d %d %d %d %d %d\n", so->codename, timecode, accelgyro[0], accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id );
+	}
+}
+
+
+void my_angle_process( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle )
+{
+	survive_default_angle_process( so, sensor_id, acode, timecode, length, angle );
+}
+
+
+void * GuiThread( void * v )
+{
+	short screenx, screeny;
+	while(1)
+	{
+		CNFGHandleInput();
+		CNFGClearFrame();
+		CNFGColor( 0xFFFFFF );
+		CNFGGetDimensions( &screenx, &screeny );
+
+		int i;
+		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 );			
+				buffertimeto[i]++;
+			}
+		}
+
+		CNFGColor( 0xffffff );
+		char caldesc[256];
+		survive_cal_get_status( ctx, caldesc, sizeof( caldesc ) );
+		CNFGPenX = 3;
+		CNFGPenY = 3;
+		CNFGDrawText( caldesc, 4 );
+
+
+		CNFGSwapBuffers();
+		OGUSleep( 10000 );
+	}
+}
+
+
+
+
+int main()
+{
+	ctx = survive_init( 1 );
+
+	survive_install_light_fn( ctx,  my_light_process );
+	survive_install_imu_fn( ctx,  my_imu_process );
+	survive_install_angle_fn( ctx, my_angle_process );
+
+	survive_cal_install( ctx );
+
+	CNFGBGColor = 0x000000;
+	CNFGDialogColor = 0x444444;
+	CNFGSetup( "Survive GUI Debug", 640, 480 );
+	OGCreateThread( GuiThread, 0 );
+	
+
+	if( !ctx )
+	{
+		fprintf( stderr, "Fatal. Could not start\n" );
+		return 1;
+	}
+
+	while(survive_poll(ctx) == 0)
+	{
+		char * lineptr;
+		size_t n;
+		lineptr = 0;
+		n = 0;
+		ssize_t gl = getline( &lineptr, &n, stdin );
+//		printf( "%d %s\n", gl, lineptr );
+
+		switch( lineptr[0] )
+		{
+			case 'I': 
+				//IMU data
+				//I WM0 -695533550 -321 1357 -3928 -16 -2 -2 0
+				break;
+			case 'R': 
+			case 'L': 
+			{
+				//Light data
+				//R X HMD -878577652 -1 6 380498 6004
+				char lhn[10];
+				char beam[10];
+				char dev[10];
+				int timecode, sensor_id, acode, timeinsweep, length;
+
+				sscanf( lineptr, "%9s %9s %9s %d %d %d %d %d\n", 
+					lhn, beam, dev, &timecode, &sensor_id, &acode, &timeinsweep, &length );
+
+				struct SurviveObject * so = 0;
+				if( strcmp( dev, "HMD" ) == 0 )
+					so = &ctx->headset;
+				if( strcmp( dev, "WM0" ) == 0 )
+					so = &ctx->watchman[0];
+				if( strcmp( dev, "WM1" ) == 0 )
+					so = &ctx->watchman[1];
+
+				if( so )
+					my_light_process( so, sensor_id, acode, timeinsweep, timecode, length );
+
+				break;
+			}
+		}
+
+		free( lineptr );
+
+        //printf( "!!!\n" );
+		//Do stuff.
+	}
+}
+
diff --git a/data_recorder.c b/data_recorder.c
index ced82c4..4fc3ffa 100644
--- a/data_recorder.c
+++ b/data_recorder.c
@@ -81,7 +81,7 @@ void my_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t ti
 {
 	survive_default_imu_process( so, accelgyro, timecode, id );
 
-return;
+//return;
 	//if( so->codename[0] == 'H' )
 	if( 1 )
 	{
@@ -132,7 +132,7 @@ void * GuiThread( void * v )
 
 int main()
 {
-	ctx = survive_init( );
+	ctx = survive_init( 0 );
 
 	survive_install_light_fn( ctx,  my_light_process );
 	survive_install_imu_fn( ctx,  my_imu_process );
diff --git a/include/survive.h b/include/survive.h
index 1b9c29c..02c5969 100644
--- a/include/survive.h
+++ b/include/survive.h
@@ -68,7 +68,7 @@ typedef void (*light_process_func)( struct SurviveObject * so, int sensor_id, in
 typedef void (*imu_process_func)( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id );
 typedef void (*angle_process_func)( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle );
 
-struct SurviveContext * survive_init();
+struct SurviveContext * survive_init( int headless );
 
 //For any of these, you may pass in 0 for the function pointer to use default behavior.
 void survive_install_info_fn( struct SurviveContext * ctx,  text_feedback_func fbp );
diff --git a/src/survive.c b/src/survive.c
index aab2ae6..473a26d 100644
--- a/src/survive.c
+++ b/src/survive.c
@@ -134,7 +134,7 @@ static int LoadConfig( struct SurviveContext * ctx, struct SurviveObject * so, i
 	return 0;
 }
 
-struct SurviveContext * survive_init()
+struct SurviveContext * survive_init( int headless )
 {
 	int r = 0;
 	struct SurviveContext * ctx = calloc( 1, sizeof( struct SurviveContext ) );
@@ -153,17 +153,20 @@ struct SurviveContext * survive_init()
 	ctx->watchman[1].ctx = ctx;
 	memcpy( ctx->watchman[1].codename, "WM1", 4 );
 
-	//USB must happen last.
-	if( r = survive_usb_init( ctx ) )
-	{
-		//TODO: Cleanup any libUSB stuff sitting around.
-		goto fail_gracefully;
-	}
-
-	//Next, pull out the config stuff.
-	if( LoadConfig( ctx, &ctx->headset, 1, 0, 0 ) ) goto fail_gracefully;
-	if( LoadConfig( ctx, &ctx->watchman[0], 2, 0, 1 ) ) { SV_INFO( "Watchman 0 config issue." ); }
-	if( LoadConfig( ctx, &ctx->watchman[1], 3, 0, 1 ) ) { SV_INFO( "Watchman 1 config issue." ); }
+    if( !headless )
+    {
+	    //USB must happen last.
+	    if( r = survive_usb_init( ctx ) )
+	    {
+		    //TODO: Cleanup any libUSB stuff sitting around.
+		    goto fail_gracefully;
+	    }
+
+	    //Next, pull out the config stuff.
+	    if( LoadConfig( ctx, &ctx->headset, 1, 0, 0 ) ) goto fail_gracefully;
+	    if( LoadConfig( ctx, &ctx->watchman[0], 2, 0, 1 ) ) { SV_INFO( "Watchman 0 config issue." ); }
+	    if( LoadConfig( ctx, &ctx->watchman[1], 3, 0, 1 ) ) { SV_INFO( "Watchman 1 config issue." ); }
+    }
 
 	ctx->headset.timebase_hz = ctx->watchman[0].timebase_hz = ctx->watchman[1].timebase_hz = 48000000;
 	ctx->headset.pulsedist_max_ticks = ctx->watchman[0].pulsedist_max_ticks = ctx->watchman[1].pulsedist_max_ticks = 500000;
@@ -253,12 +256,14 @@ void survive_install_angle_fn( struct SurviveContext * ctx,  angle_process_func
 
 void survive_close( struct SurviveContext * ctx )
 {
-	survive_usb_close( ctx );
+    if( ctx->usbctx )
+    	survive_usb_close( ctx );
 }
 
 int survive_poll( struct SurviveContext * ctx )
 {
-	survive_usb_poll( ctx );
+    if( ctx->usbctx )
+    	survive_usb_poll( ctx );
 }
 
 int survive_simple_inflate( struct SurviveContext * ctx, const char * input, int inlen, char * output, int outlen )
diff --git a/test.c b/test.c
index 17fa7f3..3256350 100644
--- a/test.c
+++ b/test.c
@@ -42,7 +42,7 @@ int main()
 	int magicon = 0;
 	double Start = OGGetAbsoluteTime();
 
-	ctx = survive_init( );
+	ctx = survive_init( 0 );
 
 	if( !ctx )
 	{
diff --git a/tools/data_server/data_server.c b/tools/data_server/data_server.c
index 29f5ead..5147d97 100644
--- a/tools/data_server/data_server.c
+++ b/tools/data_server/data_server.c
@@ -19,6 +19,7 @@
 #define MAX_CONNS 32
 
 int SocketList[MAX_CONNS];
+int droppedct[MAX_CONNS];
 
 void error(char *msg) {
 	perror(msg);
@@ -44,10 +45,17 @@ void * SendThread( void * v )
 			int ss = send( sockc, buff, rd, MSG_DONTWAIT | MSG_NOSIGNAL );
 			if( ss < rd )
 			{
-				fprintf( stderr, "Dropped %d\n", i );
-				close( SocketList[i] );
-				SocketList[i] = 0;
+                if( droppedct[i]++ > 20 )
+                {
+				    fprintf( stderr, "Dropped %d\n", i );
+				    close( SocketList[i] );
+				    SocketList[i] = 0;
+                }
 			}
+            else
+            {
+                droppedct[i] = 0;
+            }
 		}
 	}
 }
@@ -142,6 +150,7 @@ int main( int argc, char ** argv )
 			if( SocketList[il] == 0 )
 			{
 				SocketList[il] = childfd;
+                droppedct[il] = 0;
 				printf("Conn %s At %d\n", 
 					hostaddrp, il);
 				break;