* Get config from headset.

* Make pretty display.
* Improve disambiguation.

5 files changed, 125 insertions, 22 deletions

diff --git a/src/survive.c b/src/survive.c
index 726e237..b5c8012 100644
--- a/src/survive.c
+++ b/src/survive.c
@@ -5,9 +5,17 @@
 #include "survive_internal.h"
 #include <stdio.h>
 #include <stdlib.h>
+#include <jsmn.h>
 #include <string.h>
 #include <zlib.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;
+}
 
 struct SurviveContext * survive_init( void(*ff)( struct SurviveContext * ctx, const char * fault ), void(*notefunction)( struct SurviveContext * ctx, const char * note ) )
 {
@@ -32,15 +40,79 @@ struct SurviveContext * survive_init( void(*ff)( struct SurviveContext * ctx, co
 	//USB must happen last.
 	if( r = survive_usb_init( ctx ) )
 	{
+		//TODO: Cleanup any libUSB stuff sitting around.
 		return 0;
 	}
 
-#if 0
+#if 1
 	//Next, pull out the config stuff.
-	char * ct0conf;
-	int len = survive_get_config( &ct0conf, ctx, 0, 0 );
-	printf( "%d\n", len );
-	puts( ct0conf );
+	{
+		char * ct0conf = 0;
+		int len = survive_get_config( &ct0conf, ctx, 1, 0 );
+		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_ERROR("Failed to parse JSON in HMD configuration: %d\n", r);
+				return 0;
+			}
+			if (r < 1 || t[0].type != JSMN_OBJECT) {
+				SV_ERROR("Object expected in HMD configuration\n");
+				return 0;
+			}
+			for (i = 1; i < r; i++) {
+				if (jsoneq(ct0conf, &t[i], "modelPoints") == 0) {
+					int k;
+					jsmntok_t * tk = &t[i+1];
+					//printf( "%d / %d / %d / %d\n", tk->type, tk->start, tk->end, tk->size );
+					int pts = tk->size;
+
+					ctx->headset.nr_locations = 0;
+					ctx->headset.sensor_locations = malloc( sizeof( *ctx->headset.sensor_locations) * 32 * 3 );
+
+					for( k = 0; k < pts; k++ )
+					{
+						tk = &t[i+2+k*4];
+						//printf( "++%d / %d / %d / %d\n", tk->type, tk->start, tk->end, tk->size );
+				
+						float vals[3];
+						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" );
+								break;
+							}
+
+							memcpy( ctt, ct0conf + tk->start, elemlen );
+							ctt[elemlen] = 0;
+							float f = atof( ctt );
+							int id = ctx->headset.nr_locations*3+m;
+							ctx->headset.sensor_locations[id] = f;
+						}
+						ctx->headset.nr_locations++;
+					}
+				}
+			}
+		}
+		else
+		{
+			//TODO: Cleanup any remaining USB stuff.
+			return 0;
+		}
+
+	}
 #endif
 
 	//ctx->headset->photos = malloc( ctx->headset->sensors * sizeof(struct SurvivePhoto) );
diff --git a/src/survive_data.c b/src/survive_data.c
index f5d0e6b..11677e0 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -35,7 +35,8 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
 
 	if( le->type != 0xfe || le->length < 50 ) return;
 	//le->timestamp += (le->length/2);
-	if( le->length > 900 ) //Pulse longer than 18us? 
+
+	if( le->length > 2100 ) //Pulse longer indicates a sync pulse.
 	{
 		int32_t deltat = (uint32_t)le->timestamp - (uint32_t)ct->last_photo_time;
 		if( deltat > 2000 || deltat < -2000 )		//New pulse. (may be inverted)
@@ -65,8 +66,12 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
 		int32_t acode = (tpco+125)/250;
 		if( acode & 1 ) return;
 		acode>>=1;
+		acode -= 6;
 
-		survive_light_process( so, le->sensor_id, acode, offset_from, le->timestamp );
+		if( offset_from < 380000 )
+		{
+			survive_light_process( so, le->sensor_id, acode, offset_from, le->timestamp );
+		}
 	}
 	else
 	{
diff --git a/src/survive_internal.h b/src/survive_internal.h
index 8096b8c..d6168b2 100644
--- a/src/survive_internal.h
+++ b/src/survive_internal.h
@@ -26,6 +26,8 @@
 //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
@@ -72,6 +74,9 @@ struct SurviveObject
 	int8_t  charging:1;
 	int8_t  ison:1;
 	int sensors;
+
+	int nr_locations;
+	SV_FLOAT * sensor_locations;
 };
 
 struct SurviveContext
diff --git a/src/survive_process.c b/src/survive_process.c
index a4680af..139ae2f 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -3,16 +3,42 @@
 
 #include "survive_internal.h"
 
+
+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  )
 {
+	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-170000)/100;
+		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] = 480-(timeinsweep-140000)/100;
+		//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"
-	//printf( "L %s %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep );
 }
 
 void survive_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id )
 {
-	if( so->codename[0] != 'H' )
+	//if( so->codename[0] == 'H' )
+	if( 0 )
 	{
-		//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 );
+		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 );
 	}
 }
diff --git a/src/survive_usb.c b/src/survive_usb.c
index cfcef17..affcdd5 100644
--- a/src/survive_usb.c
+++ b/src/survive_usb.c
@@ -298,8 +298,7 @@ int survive_usb_poll( struct SurviveContext * ctx )
 	return r;
 }
 
-#if 0
-//XXX THIS DOES NOT WORK!!! WHY???
+
 int survive_get_config( char ** config, struct SurviveContext * ctx, int devno, int iface )
 {
 	int i, ret, count = 0, size = 0;
@@ -327,10 +326,10 @@ int survive_get_config( char ** config, struct SurviveContext * ctx, int devno,
 		}
 
 		size = cfgbuff[1];
-		printf( "Tag: " );
-		for( i = 0; i < 64; i++ )
-			printf( "%02x ", cfgbuff[i] );
-		printf( "ret: %d %d\n", ret, size );
+//		printf( "Tag: " );
+//		for( i = 0; i < 64; i++ )
+//			printf( "%02x ", cfgbuff[i] );
+//		printf( "ret: %d %d\n", ret, size );
 
 		if( !size ) break;
 
@@ -365,13 +364,9 @@ int survive_get_config( char ** config, struct SurviveContext * ctx, int devno,
 		return -5;
 	}
 
-	config = malloc( len + 1 );
-	memcpy( config, uncompressed_data, len );
+	*config = malloc( len + 1 );
+	memcpy( *config, uncompressed_data, len );
 	return len;
 }
-#endif
-
-
-