Merge branch 'master' of https://github.com/cnlohr/libsurvive

5 files changed, 121 insertions, 122 deletions

diff --git a/src/survive.c b/src/survive.c
index edcbb86..773f02a 100644
--- a/src/survive.c
+++ b/src/survive.c
@@ -17,13 +17,29 @@ static int jsoneq(const char *json, jsmntok_t *tok, const char *s) {
 	return -1;
 }
 
-struct SurviveContext * survive_init( void(*ff)( struct SurviveContext * ctx, const char * fault ), void(*notefunction)( struct SurviveContext * ctx, const char * note ) )
+
+static void survivefault( struct SurviveContext * ctx, const char * fault )
+{
+	fprintf( stderr, "Error: %s\n", fault );
+	exit( -1 );
+}
+
+static void survivenote( struct SurviveContext * ctx, const char * fault )
+{
+	fprintf( stderr, "Info: %s\n", fault );
+}
+
+
+struct SurviveContext * survive_init()
 {
 	int r = 0;
-	struct SurviveContext * ctx = calloc( 1, sizeof( struct SurviveContext  ) );
+	struct SurviveContext * ctx = calloc( 1, sizeof( struct SurviveContext ) );
+
+	ctx->faultfunction = survivefault;
+	ctx->notefunction = survivenote;
 
-	ctx->faultfunction = ff;
-	ctx->notefunction = notefunction;
+	ctx->lightproc = survive_default_light_process;
+	ctx->imuproc = survive_default_imu_process;
 
 	ctx->headset.sensors = 32;
 	ctx->headset.ctx = ctx;
@@ -44,8 +60,6 @@ struct SurviveContext * survive_init( void(*ff)( struct SurviveContext * ctx, co
 		return 0;
 	}
 
-
-#if 1
 	//Next, pull out the config stuff.
 	{
 		char * ct0conf = 0;
@@ -123,12 +137,42 @@ struct SurviveContext * survive_init( void(*ff)( struct SurviveContext * ctx, co
 		}
 
 	}
-#endif
 
 	//ctx->headset->photos = malloc( ctx->headset->sensors * sizeof(struct SurvivePhoto) );
 	return ctx;
 }
 
+void survive_install_info_fn( struct SurviveContext * ctx,  text_feedback_fnptr fbp )
+{
+	if( fbp )
+		ctx->notefunction = fbp;
+	else
+		ctx->notefunction = survivenote;
+}
+
+void survive_install_error_fn( struct SurviveContext * ctx,  text_feedback_fnptr fbp )
+{
+	if( fbp )
+		ctx->faultfunction = fbp;
+	else
+		ctx->faultfunction = survivefault;
+}
+
+void survive_install_light_fn( struct SurviveContext * ctx, light_process_func fbp )
+{
+	if( fbp )
+		ctx->lightproc = fbp;
+	else
+		ctx->lightproc = survive_default_light_process;
+}
+
+void survive_install_imu_fn( struct SurviveContext * ctx,  imu_process_func fbp )
+{
+	if( fbp )
+		ctx->imuproc = fbp;
+	else
+		ctx->imuproc = survive_default_imu_process;
+}
 
 void survive_close( struct SurviveContext * ctx )
 {
diff --git a/src/survive_data.c b/src/survive_data.c
index 11677e0..88c9e7d 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -41,11 +41,14 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
 		int32_t deltat = (uint32_t)le->timestamp - (uint32_t)ct->last_photo_time;
 		if( deltat > 2000 || deltat < -2000 )		//New pulse. (may be inverted)
 		{
-			ct->last_photo_time = le->timestamp;
-			ct->total_photo_time = 0;
-			ct->total_photos = 0;
-			ct->total_pulsecode_time = 0;
-			survive_light_process( so, le->sensor_id, -1, 0, le->timestamp );
+			if( le->timestamp - ct->last_photo_time > 80000 )
+			{
+				ct->last_photo_time = le->timestamp;
+				ct->total_photo_time = 0;
+				ct->total_photos = 0;
+				ct->total_pulsecode_time = 0;
+				ct->lightproc( so, le->sensor_id, -1, 0, le->timestamp, deltat );
+			}
 		}
 		else
 		{
@@ -54,7 +57,7 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
 			ct->total_photos++;
 		}
 	}
-	else if( le->length < 900 && le->length > 50 && ct->total_photos )
+	else if( le->length < 1200 && le->length > 40 && ct->total_photos )
 	{
 		int32_t dl = (ct->total_photo_time/ct->total_photos);
 		int32_t tpco = (ct->total_pulsecode_time/ct->total_photos);
@@ -70,7 +73,7 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
 
 		if( offset_from < 380000 )
 		{
-			survive_light_process( so, le->sensor_id, acode, offset_from, le->timestamp );
+			ct->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length );
 		}
 	}
 	else
@@ -155,7 +158,7 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata )
 	if( ( ( type & 0xe8 ) == 0xe8 ) || doimu ) //Hmm, this looks kind of yucky... we can get e8's that are accelgyro's but, cleared by first propset.
 	{
 		propset |= 2;
-		survive_imu_process( w, (int16_t *)&readdata[1], (time1<<24)|(time2<<16)|readdata[0], 0 );
+		w->ctx->imuproc( w, (int16_t *)&readdata[1], (time1<<24)|(time2<<16)|readdata[0], 0 );
 		int16_t * k = (int16_t *)readdata+1;
 		//printf( "Match8 %d %d %d %d %d %3d %3d\n", qty, k[0], k[1], k[2], k[3], k[4], k[5] );
 		readdata += 13; qty -= 13;
@@ -175,16 +178,6 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata )
 		readdata--;
 		*readdata = type;
 
-#if 1
-		//             good        good          maybe?         probably wrong
-		printf( "POST %d: %4d (%02x%02x) - ", propset, qty, time1, time2 );
-		for( i = 0; i < qty + 4; i++ )
-		{
-			printf( "%02x ", readdata[i] );
-		}
-		printf("\n");
-#endif
-
 		uint8_t * end = &readdata[qty];
 		uint32_t mytime = (end[1] << 0)|(end[2] << 8)|(end[3] << 16);
 
@@ -286,7 +279,8 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata )
 		int i;
 		for( i = lese-1; i >= 0; i-- )
 		{
-			printf( "%d: %d [%d]\n", les[i].sensor_id, les[i].length, les[i].timestamp );
+			//printf( "%d: %d [%d]\n", les[i].sensor_id, les[i].length, les[i].timestamp );
+			handle_lightcap( w, &les[i] );
 		}
 
 		return;
@@ -342,14 +336,6 @@ void survive_data_cb( struct SurviveUSBInterface * si )
 		//printf( "%d -> ", size );
 		for( i = 0; i < 3; i++ )
 		{
-/*
-			for( i = 0; i < 17; i++ )
-			{
-				printf( "%02x ", readdata[i] );
-			}
-			printf( "\n" );
-*/
-			//handle_lightdata( (struct LightpulseStructure *)readdata );
 			int16_t * acceldata = (int16_t*)readdata;
 			readdata += 12;
 			uint32_t timecode = POP4;
@@ -360,7 +346,7 @@ void survive_data_cb( struct SurviveUSBInterface * si )
 			if( cd > 0 )
 			{
 				ctx->oldcode = code;
-				survive_imu_process( &ctx->headset, acceldata, timecode, code );
+				ctx->imuproc( &ctx->headset, acceldata, timecode, code );
 			}
 		}
 
diff --git a/src/survive_internal.h b/src/survive_internal.h
index d6168b2..ac378a2 100644
--- a/src/survive_internal.h
+++ b/src/survive_internal.h
@@ -19,6 +19,7 @@
 #include <stdint.h>
 #include <libusb-1.0/libusb.h>
 #include <zlib.h>
+#include <survive.h>
 
 #define SV_INFO( x... ) { char stbuff[1024]; sprintf( stbuff, x ); ctx->notefunction( ctx, stbuff ); }
 #define SV_ERROR( x... ) { char stbuff[1024]; sprintf( stbuff, x ); ctx->faultfunction( ctx, stbuff ); }
@@ -26,8 +27,6 @@
 //XXX TODO This one needs to be rewritten.
 #define SV_KILL()		exit(0)
 
-#define SV_FLOAT  		double
-
 #define USB_DEV_HMD			0
 #define USB_DEV_LIGHTHOUSE	1
 #define USB_DEV_WATCHMAN1	2
@@ -61,33 +60,20 @@ struct SurviveUSBInterface
 	const char * hname;			//human-readable names
 };
 
-
-struct SurviveObject
-{
-	struct SurviveContext * ctx;
-	char    codename[4];
-	int16_t buttonmask;
-	int16_t axis1;
-	int16_t axis2;
-	int16_t axis3;
-	int8_t  charge;
-	int8_t  charging:1;
-	int8_t  ison:1;
-	int sensors;
-
-	int nr_locations;
-	SV_FLOAT * sensor_locations;
-};
+//This is defined in survive.h
+struct SurviveObject;
 
 struct SurviveContext
 {
 	//USB Subsystem
     struct libusb_context* usbctx;
-	void(*faultfunction)( struct SurviveContext * ctx, const char * fault );
-	void(*notefunction)( struct SurviveContext * ctx, const char * fault );
 	struct libusb_device_handle * udev[MAX_USB_DEVS];
 	struct SurviveUSBInterface uiface[MAX_INTERFACES];
 
+	text_feedback_fnptr faultfunction;
+	text_feedback_fnptr notefunction;
+	light_process_func lightproc;
+	imu_process_func imuproc;
 
 	//Flood info, for calculating which laser is currently sweeping.
 	int8_t oldcode;
@@ -113,13 +99,8 @@ int survive_get_config( char ** config, struct SurviveContext * ctx, int devno,
 void survive_data_cb( struct SurviveUSBInterface * si );
 
 //Accept higher-level data.
-void survive_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode );
-void survive_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id );
-
-
-//Util
-int survive_simple_inflate( struct SurviveContext * ctx, const char * input, int inlen, char * output, int outlen );
-
+void survive_default_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length );
+void survive_default_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id );
 
 #endif
 
diff --git a/src/survive_process.c b/src/survive_process.c
index 268fdfc..d3a8c4a 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -8,56 +8,14 @@ int bufferpts[32*2];
 char buffermts[32*128];
 int buffertimeto[32];
 
-void survive_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode  )
+void survive_default_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length  )
 {
-	if( acode == -1 ) return;
-
-	if( acode == 0 || acode == 2 ) //data = 0
-	{
-		printf( "L, X, %s, %d, %d, %d, %d\n", so->codename, timecode, sensor_id, acode, timeinsweep );
-		bufferpts[sensor_id*2+0] = (timeinsweep-100000)/500;
-		buffertimeto[sensor_id] = 0;
-		//printf( "X: %d\n",bufferpts[sensor_id*2+0] );
-			//480-(timeinsweep)/1000; // Full scan
-	}
-	if( acode == 1 || acode == 3 ) //data = 1
-	{
-		printf( "L, Y, %s, %d, %d, %d, %d\n", so->codename, timecode, sensor_id, acode, timeinsweep );
-		bufferpts[sensor_id*2+1] = (timeinsweep-100000)/500;
-		//printf( "Y: %d\n",bufferpts[sensor_id*2+1] );
-		buffertimeto[sensor_id] = 0;
-
-			//480-(timeinsweep)/1000; //Full scan
-	}
-
-
-	if( acode == 4 || acode == 6 ) //data = 0
-	{
-		printf( "R, X, %s, %d, %d, %d, %d\n", so->codename, timecode, sensor_id, acode, timeinsweep );
-		bufferpts[sensor_id*2+0] = (timeinsweep-100000)/500;
-		buffertimeto[sensor_id] = 0;
-		//printf( "X: %d\n",bufferpts[sensor_id*2+0] );
-			//480-(timeinsweep)/1000; // Full scan
-	}
-	if( acode == 5 || acode == 7 ) //data = 1
-	{
-		printf( "R, Y, %s, %d, %d, %d, %d\n", so->codename, timecode, sensor_id, acode, timeinsweep );
-		bufferpts[sensor_id*2+1] = (timeinsweep-100000)/500;
-		//printf( "Y: %d\n",bufferpts[sensor_id*2+1] );
-		buffertimeto[sensor_id] = 0;
-
-			//480-(timeinsweep)/1000; //Full scan
-	}
-
-
-	//timeinsweep = 200,000 1/48,000,000ths of a second is "center-of-image"
+	//TODO: Writeme!
 }
 
-void survive_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id )
+void survive_default_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id )
 {
-	//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 );
-	}
+	//TODO: Writeme!
 }
+
+
diff --git a/src/survive_usb.c b/src/survive_usb.c
index 07e5cb4..4f341ec 100644
--- a/src/survive_usb.c
+++ b/src/survive_usb.c
@@ -233,12 +233,22 @@ int survive_usb_init( struct SurviveContext * ctx )
 
 	SV_INFO( "All devices attached." );
 
-#if 1
+
+	//libUSB initialized.  Continue.
+	return 0;
+}
+
+int survive_usb_send_magic(struct SurviveContext * ctx, int turnon )
+{
+	int r;
+
+
+	if( turnon )
 	{
 		//Magic from vl_magic.h, originally copywritten under LGPL. 
 		// * Copyright (C) 2013 Fredrik Hultin
 		// * Copyright (C) 2013 Jakob Bornecrantz
-
+#if 0
 		static uint8_t vive_magic_power_on[] = {
 			0x04, 0x78, 0x29, 0x38, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x01,
 			0xa8, 0x0d, 0x76, 0x00, 0x40, 0xfc, 0x01, 0x05, 0xfa, 0xec, 0xd1, 0x6d, 0x00,
@@ -246,15 +256,14 @@ int survive_usb_init( struct SurviveContext * ctx )
 			0x01, 0x05, 0x2c, 0xb0, 0x2e, 0x65, 0x7a, 0x0d, 0x76, 0x00, 0x68, 0x54, 0x72,
 			0x00, 0x18, 0x54, 0x72, 0x00, 0x00, 0x6a, 0x72, 0x00, 0x00, 0x00, 0x00,
 		};
-
-		//From actual use. ((THIS SEEMS TO BREAK IT))
-/*
+#else
+		//From actual steam.
 		static uint8_t vive_magic_power_on[64] = {   0x04, 0x78, 0x29, 0x38,
 			0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00,	 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 			0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 			0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  0x00, 0x7a, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 			0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
-*/
+#endif
 		r = update_feature_report( ctx->udev[USB_DEV_HMD], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) );
 		SV_INFO( "UCR: %d", r );
 		if( r != sizeof( vive_magic_power_on ) ) return 5;
@@ -274,17 +283,38 @@ int survive_usb_init( struct SurviveContext * ctx )
 			usleep( 1000 );
 		}
 #endif
+		SV_INFO( "Powered unit on." );
 	}
-#endif
+	else
+	{
 
-	SV_INFO( "Powered unit on." );
+		static uint8_t vive_magic_power_off1[] = {
+			0x04, 0x78, 0x29, 0x38, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00,
+			0x30, 0x05, 0x77, 0x00, 0x30, 0x05, 0x77, 0x00, 0x6c, 0x4d, 0x37, 0x65, 0x40,
+			0xf9, 0x33, 0x00, 0x04, 0xf8, 0xa3, 0x04, 0x04, 0x00, 0x00, 0x00, 0x70, 0xb0,
+			0x72, 0x00, 0xf4, 0xf7, 0xa3, 0x04, 0x7c, 0xf8, 0x33, 0x00, 0x0c, 0xf8, 0xa3,
+			0x04, 0x0a, 0x6e, 0x29, 0x65, 0x24, 0xf9, 0x33, 0x00, 0x00, 0x00, 0x00,
+		};
 
+		static uint8_t vive_magic_power_off2[] = {
+			0x04, 0x78, 0x29, 0x38, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00,
+			0x30, 0x05, 0x77, 0x00, 0xe4, 0xf7, 0x33, 0x00, 0xe4, 0xf7, 0x33, 0x00, 0x60,
+			0x6e, 0x72, 0x00, 0xb4, 0xf7, 0x33, 0x00, 0x04, 0x00, 0x00, 0x00, 0x70, 0xb0,
+			0x72, 0x00, 0x90, 0xf7, 0x33, 0x00, 0x7c, 0xf8, 0x33, 0x00, 0xd0, 0xf7, 0x33,
+			0x00, 0x3c, 0x68, 0x29, 0x65, 0x24, 0xf9, 0x33, 0x00, 0x00, 0x00, 0x00,
+		};
 
+//		r = update_feature_report( ctx->udev[USB_DEV_HMD], 0, vive_magic_power_off1, sizeof( vive_magic_power_off1 ) );
+//		SV_INFO( "UCR: %d", r );
+//		if( r != sizeof( vive_magic_power_off1 ) ) return 5;
 
-	//libUSB initialized.  Continue.
-	return 0;
-}
+		r = update_feature_report( ctx->udev[USB_DEV_HMD], 0, vive_magic_power_off2, sizeof( vive_magic_power_off2 ) );
+		SV_INFO( "UCR: %d", r );
+		if( r != sizeof( vive_magic_power_off2 ) ) return 5;
 
+	}
+
+}
 
 void survive_usb_close( struct SurviveContext * t )
 {