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

12 files changed, 512 insertions, 336 deletions

diff --git a/src/survive.c b/src/survive.c
index 9bc1a2c..efa5d82 100644
--- a/src/survive.c
+++ b/src/survive.c
@@ -3,7 +3,6 @@
 
 #include <survive.h>
 #include "survive_internal.h"
-#include "survive_driverman.h"
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
@@ -21,11 +20,11 @@ static void survivenote( struct SurviveContext * ctx, const char * fault )
 }
 
 
-struct SurviveContext * survive_init( int headless )
+SurviveContext * survive_init( int headless )
 {
 	int r = 0;
 	int i = 0;
-	struct SurviveContext * ctx = calloc( 1, sizeof( struct SurviveContext ) );
+	SurviveContext * ctx = calloc( 1, sizeof( SurviveContext ) );
 
 	ctx->faultfunction = survivefault;
 	ctx->notefunction = survivenote;
@@ -36,7 +35,6 @@ struct SurviveContext * survive_init( int headless )
 
 	const char * DriverName;
 	while( ( DriverName = GetDriverNameMatching( "DriverReg", i++ ) ) )
-
 	{
 		DeviceDriver dd = GetDriver( DriverName );
 		printf( "Loading driver %s (%p) (%d)\n", DriverName, dd, i );
@@ -44,10 +42,34 @@ struct SurviveContext * survive_init( int headless )
 		printf( "Driver %s reports status %d\n", DriverName, r );
 	}
 
+	i = 0;
+	const char * PreferredPoser = "PoserDummy"; //config_read_str( cg, "DefualtPoser", "PoserDummy" ); /XXX Axlecrusher, can you add config stuff for this?
+	PoserCB PreferredPoserCB = 0;
+	const char * FirstPoser = 0;
+	printf( "Available posers:\n" );
+	while( ( DriverName = GetDriverNameMatching( "Poser", i++ ) ) )
+	{
+		PoserCB p = GetDriver( DriverName );
+		if( !PreferredPoserCB ) PreferredPoserCB = p;
+		int ThisPoser = strcmp( DriverName, PreferredPoser ) == 0;
+		printf( "\t%c%s\n", ThisPoser?'*':' ', DriverName );
+		if( ThisPoser ) PreferredPoserCB = p;
+	}
+	printf( "Totals %d posers.  Using selected poser (or first!).\n", i-1 );
+	if( !PreferredPoserCB )
+	{
+		SV_ERROR( "Error.  Cannot find any valid poser." );
+	}
+
+	for( i = 0; i < ctx->objs_ct; i++ )
+	{
+		ctx->objs[i]->PoserFn = PreferredPoserCB;
+	}
+
 	return ctx;
 }
 
-void survive_install_info_fn( struct SurviveContext * ctx,  text_feedback_func fbp )
+void survive_install_info_fn( SurviveContext * ctx,  text_feedback_func fbp )
 {
 	if( fbp )
 		ctx->notefunction = fbp;
@@ -55,7 +77,7 @@ void survive_install_info_fn( struct SurviveContext * ctx,  text_feedback_func f
 		ctx->notefunction = survivenote;
 }
 
-void survive_install_error_fn( struct SurviveContext * ctx,  text_feedback_func fbp )
+void survive_install_error_fn( SurviveContext * ctx,  text_feedback_func fbp )
 {
 	if( fbp )
 		ctx->faultfunction = fbp;
@@ -63,7 +85,7 @@ void survive_install_error_fn( struct SurviveContext * ctx,  text_feedback_func
 		ctx->faultfunction = survivefault;
 }
 
-void survive_install_light_fn( struct SurviveContext * ctx, light_process_func fbp )
+void survive_install_light_fn( SurviveContext * ctx, light_process_func fbp )
 {
 	if( fbp )
 		ctx->lightproc = fbp;
@@ -71,7 +93,7 @@ void survive_install_light_fn( struct SurviveContext * ctx, light_process_func f
 		ctx->lightproc = survive_default_light_process;
 }
 
-void survive_install_imu_fn( struct SurviveContext * ctx,  imu_process_func fbp )
+void survive_install_imu_fn( SurviveContext * ctx,  imu_process_func fbp )
 {
 	if( fbp )
 		ctx->imuproc = fbp;
@@ -80,7 +102,7 @@ void survive_install_imu_fn( struct SurviveContext * ctx,  imu_process_func fbp
 }
 
 
-void survive_install_angle_fn( struct SurviveContext * ctx,  angle_process_func fbp )
+void survive_install_angle_fn( SurviveContext * ctx,  angle_process_func fbp )
 {
 	if( fbp )
 		ctx->angleproc = fbp;
@@ -88,15 +110,15 @@ void survive_install_angle_fn( struct SurviveContext * ctx,  angle_process_func
 		ctx->angleproc = survive_default_angle_process;
 }
 
-int survive_add_object( struct SurviveContext * ctx, struct SurviveObject * obj )
+int survive_add_object( SurviveContext * ctx, SurviveObject * obj )
 {
 	int oldct = ctx->objs_ct;
-	ctx->objs = realloc( ctx->objs, sizeof( struct SurviveObject * ) * (oldct+1) );
+	ctx->objs = realloc( ctx->objs, sizeof( SurviveObject * ) * (oldct+1) );
 	ctx->objs[oldct] = obj;
 	ctx->objs_ct = oldct+1;
 }
 
-void survive_add_driver( struct SurviveContext * ctx, void * payload, DeviceDriverCb poll, DeviceDriverCb close, DeviceDriverMagicCb magic )
+void survive_add_driver( SurviveContext * ctx, void * payload, DeviceDriverCb poll, DeviceDriverCb close, DeviceDriverMagicCb magic )
 {
 	int oldct = ctx->driver_ct;
 	ctx->drivers = realloc( ctx->drivers, sizeof( void * ) * (oldct+1) );
@@ -110,7 +132,7 @@ void survive_add_driver( struct SurviveContext * ctx, void * payload, DeviceDriv
 	ctx->driver_ct = oldct+1;
 }
 
-int survive_send_magic( struct SurviveContext * ctx, int magic_code, void * data, int datalen )
+int survive_send_magic( SurviveContext * ctx, int magic_code, void * data, int datalen )
 {
 	int oldct = ctx->driver_ct;
 	int i;
@@ -120,7 +142,7 @@ int survive_send_magic( struct SurviveContext * ctx, int magic_code, void * data
 	}	
 }
 
-void survive_close( struct SurviveContext * ctx )
+void survive_close( SurviveContext * ctx )
 {
 	const char * DriverName;
 	int r = 0;
@@ -134,6 +156,14 @@ void survive_close( struct SurviveContext * ctx )
 
 	int oldct = ctx->driver_ct;
 	int i;
+
+	for( i = 0; i < ctx->objs_ct; i++ )
+	{
+		PoserData pd;
+		pd.pt = POSERDATA_DISASSOCIATE;
+		if( ctx->objs[i]->PoserFn ) ctx->objs[i]->PoserFn( ctx->objs[i], &pd );
+	}
+
 	for( i = 0; i < oldct; i++ )
 	{
 		ctx->drivercloses[i]( ctx, ctx->drivers[i] );
diff --git a/src/survive_cal.c b/src/survive_cal.c
index 2ed1986..2acf51c 100644
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -15,6 +15,8 @@
 #include <sys/stat.h>
 #include <sys/types.h>
 
+#include "survive_config.h"
+
 #define PTS_BEFORE_COMMON 32
 #define NEEDED_COMMON_POINTS 10
 #define NEEDED_TIMES_OF_COMMON 5
@@ -25,8 +27,8 @@ static void reset_calibration( struct SurviveCalData * cd );
 
 void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet)
 {
-	struct SurviveContext * ctx = (struct SurviveContext*)(ct->user);
-	struct SurviveCalData * cd = ctx->calptr;
+	SurviveContext * ctx = (SurviveContext*)(ct->user);
+	SurviveCalData * cd = ctx->calptr;
 	int id = ct->user1;
 
 	SV_INFO( "Got OOTX packet %d %p", id, cd );
@@ -34,7 +36,7 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet)
 	lighthouse_info_v6 v6;
 	init_lighthouse_info_v6(&v6, packet->data);
 
-	struct BaseStationData * b = &ctx->bsd[id];
+	BaseStationData * b = &ctx->bsd[id];
 	//print_lighthouse_info_v6(&v6);
 
 	b->BaseStationID = v6.id;
@@ -49,6 +51,9 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet)
 	b->fcalgibmag[0] = v6.fcal_0_gibmag;
 	b->fcalgibmag[1] = v6.fcal_1_gibmag;
 	b->OOTXSet = 1;
+
+	config_set_lighthouse(b,id);
+//	config_save("config.json");
 }
 
 int survive_cal_get_status( struct SurviveContext * ctx, char * description, int description_length )
diff --git a/src/survive_cal.h b/src/survive_cal.h
index bb4eb32..dd2a1e2 100644
--- a/src/survive_cal.h
+++ b/src/survive_cal.h
@@ -2,6 +2,8 @@
 
 // All OOTX code was written by J. Allen. Rest of the code is probably mostly CNLohr.
 
+//XXX XXX XXX Warning:  This subsystem will likely be mostly re-written.
+
 #ifndef _SURVIVE_CAL_H
 #define _SURVIVE_CAL_H
 
@@ -21,21 +23,21 @@
 #include "ootx_decoder.h"
 #include "survive_internal.h"
 
-void survive_cal_install( struct SurviveContext * ctx );
-int survive_cal_get_status( struct SurviveContext * ctx, char * description, int description_length );
+void survive_cal_install( SurviveContext * ctx );
+int survive_cal_get_status( SurviveContext * ctx, char * description, int description_length );
 
 //void survive_cal_teardown( struct SurviveContext * ctx );
 
 //Called from survive_default_light_process
-void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length  );
-void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle );
+void survive_cal_light( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length  );
+void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle );
 
 #define MAX_SENSORS_TO_CAL 96
 #define DRPTS 1024
 #define MAX_CAL_PT_DAT (MAX_SENSORS_TO_CAL*NUM_LIGHTHOUSES*2)
 struct SurviveCalData
 {
-	struct SurviveContext * ctx;
+	SurviveContext * ctx;
 	//OOTX Data is sync'd off of the sync pulses coming from the lighthouses.
 	ootx_decoder_context ootx_decoders[NUM_LIGHTHOUSES];
 
@@ -57,7 +59,7 @@ struct SurviveCalData
 
 	int senid_of_checkpt; //This is a point on a watchman that can be used to check the lh solution.
 
-	struct SurviveObject * hmd;
+	SurviveObject * hmd;
 
 	//Stage:
 	// 0: Idle
@@ -68,7 +70,7 @@ struct SurviveCalData
 
 //The following function is not included in the core survive_cal and must be compiled from a camfind file.
 //It should use data for stage 4 and report if it found the 
-int survive_cal_lhfind( struct SurviveCalData * cd );
+int survive_cal_lhfind( SurviveCalData * cd );
 
 
 #endif
diff --git a/src/survive_cal_lhfind.c b/src/survive_cal_lhfind.c
index e1e5fc9..a1bb2cc 100644
--- a/src/survive_cal_lhfind.c
+++ b/src/survive_cal_lhfind.c
@@ -7,13 +7,13 @@
 #define MIN_HITS_FOR_VALID 10
 
 
-FLT static RunOpti(  struct SurviveCalData * cd, int lh, int print, FLT * LighthousePos, FLT * LighthouseQuat );
+FLT static RunOpti(  SurviveCalData * cd, int lh, int print, FLT * LighthousePos, FLT * LighthouseQuat );
 
 //Values used for RunTest()
 
-int survive_cal_lhfind( struct SurviveCalData * cd )
+int survive_cal_lhfind( SurviveCalData * cd )
 {
-	struct SurviveContext * ctx = cd->ctx;
+	SurviveContext * ctx = cd->ctx;
 	int cycle, i;
 	int lh = 0;
 	FLT dx, dy, dz;
@@ -129,7 +129,7 @@ int survive_cal_lhfind( struct SurviveCalData * cd )
 			fullrange *= 0.25;
 		}
 
-		if( beste > 0.005 )
+		if( beste > 0.01 )
 		{
 			//Error too high
 			SV_ERROR( "LH: %d / Best E %f Error too high\n", lh, beste );
@@ -137,8 +137,8 @@ int survive_cal_lhfind( struct SurviveCalData * cd )
 		}
 
 		cd->ctx->bsd[lh].PositionSet = 1;
-		copy3d( cd->ctx->bsd[lh].Position, LighthousePos );
-		quatcopy( cd->ctx->bsd[lh].Quaternion, LighthouseQuat );
+		copy3d( cd->ctx->bsd[lh].Pose.Pos, LighthousePos );
+		quatcopy( cd->ctx->bsd[lh].Pose.Rot, LighthouseQuat );
 	}
 
 	return 0; //Return 0 if success.
@@ -149,14 +149,14 @@ int survive_cal_lhfind( struct SurviveCalData * cd )
 
 
 
-static FLT RunOpti( struct SurviveCalData * cd, int lh, int print, FLT * LighthousePos, FLT * LighthouseQuat )
+static FLT RunOpti( SurviveCalData * cd, int lh, int print, FLT * LighthousePos, FLT * LighthouseQuat )
 {
 	int i, p;
 	FLT UsToTarget[3];
 	FLT LastUsToTarget[3];
 	FLT mux = .9;
 	quatsetnone( LighthouseQuat );
-	struct SurviveObject * hmd = cd->hmd;
+	SurviveObject * hmd = cd->hmd;
 	FLT * hmd_points  = hmd->sensor_locations;
 	FLT * hmd_normals = hmd->sensor_normals;
 
diff --git a/src/survive_config.c b/src/survive_config.c
index ed3f6cd..352a15b 100644
--- a/src/survive_config.c
+++ b/src/survive_config.c
@@ -1,174 +1,359 @@
+// (C) 2017 <>< Joshua Allen, Under MIT/x11 License.
 #include <string.h>
 #include <assert.h>
 #include "survive_config.h"
+#include <json_helpers.h>
+
+#include <errno.h>
 
 #define MAX_CONFIG_ENTRIES 100
+#define MAX_LIGHTHOUSES 2
 
-config_val config_values[MAX_CONFIG_ENTRIES];
-static uint16_t used_entries = 0;
+config_group global_config_values;
+config_group lh_config[MAX_LIGHTHOUSES]; //lighthouse configs
 
-static FILE *config_file = NULL;
+//static uint16_t used_entries = 0;
 
-void config_init() {
-	uint16_t i = 0;
-	for (i=0;i<MAX_CONFIG_ENTRIES;++i) {
-		config_values[i].str = NULL;
-		config_values[i].tag = NULL;
-	}
+//static FILE *config_file = NULL;
+const FLT* config_set_float_a(config_group *cg, const char *tag, const FLT* values, uint8_t count);
 
-	used_entries = 0;
+void init_config_entry(config_entry* ce) {
+	ce->data = NULL;
+	ce->tag = NULL;
+	ce->type = CONFIG_UNKNOWN;
+	ce->elements = 0;
 }
 
-void write_float(char* tag, FLT x) {
-	fprintf(config_file, "\"%s\":\"%f\"\n", tag, x);
+void destroy_config_entry(config_entry* ce) {
+	if (ce->tag!=NULL) free(ce->tag);
+	if (ce->data!=NULL) free(ce->data);
 }
 
-void write_float_a(char* tag, FLT *x, uint8_t count) {
-	uint8_t i = 0;
-	char idx[4];
+void init_config_group(config_group *cg, uint16_t count) {
+	uint16_t i = 0;
+	cg->config_entries = malloc(count*sizeof(config_entry));
+	cg->used_entries = 0;
+	cg->max_entries = count;
+
 	for (i=0;i<count;++i) {
-		sprintf(idx,"%d",i);
-		fprintf(config_file, "\"%s%s\":\"%f\"\n", tag,idx, x);
+		init_config_entry(cg->config_entries+i);
 	}
-	fprintf(config_file, "\"%s\":\"%f\"\n", tag, x);
 }
 
-void write_uint32(char* tag, uint32_t x) {
-	fprintf(config_file, "\"%s\":\"%d\"\n", tag, x);
-}
+void resize_config_group(config_group *cg, uint16_t count) {
+	uint16_t i = 0;
+
+	if (count > cg->max_entries) {
+		config_entry* ptr = realloc(cg->config_entries, sizeof(config_entry)*count);
+		assert(ptr!=NULL);
+
+		cg->config_entries = ptr;
 
-void config_open(const char* path, const char* mode) {
-	config_file = fopen(path, mode);
+		for (i=cg->max_entries;i<count;++i) {
+			init_config_entry(cg->config_entries+i);
+		}
+
+		cg->max_entries = count;
+	}
 }
 
-void config_close() {
-	fclose(config_file);
+void config_init() {
+	uint16_t i = 0;
+	init_config_group(&global_config_values, MAX_CONFIG_ENTRIES);
+	for(i=0;i<MAX_LIGHTHOUSES;i++) {
+		init_config_group(lh_config+i, 9);
+	}
 }
 
-void config_write_lighthouse(struct BaseStationData* bsd, uint8_t length) {
-	uint8_t i = 0;
 
-	for (i=0;i<length; ++i) {
-		write_uint32("id", bsd[i].BaseStationID);
-		write_float_a("position", bsd[i].Position, 3);
-		write_float_a("quaternion", bsd[i].Quaternion, 4);
-		write_float_a("quaternion", bsd[i].Quaternion, 4);
-		write_float_a("fcalphase", bsd[i].fcalphase, 2);
-		write_float_a("fcaltilt", bsd[i].fcaltilt,2);
-		write_float_a("fcalcurve", bsd[i].fcalcurve,2);
-		write_float_a("fcalgibpha", bsd[i].fcalgibpha,2);
-		write_float_a("fcalgibmag", bsd[i].fcalgibmag,2);
-	}
+void config_set_lighthouse(BaseStationData* bsd, uint8_t idx) {
+	config_group *cg = lh_config+idx;
+	config_set_uint32(cg,"index", idx);
+	config_set_uint32(cg,"id", bsd->BaseStationID);
+	config_set_float_a(cg,"pose", &bsd->Pose.Pos[0], 7);
+	config_set_float_a(cg,"fcalphase", bsd->fcalphase, 2);
+	config_set_float_a(cg,"fcaltilt", bsd->fcaltilt,2);
+	config_set_float_a(cg,"fcalcurve", bsd->fcalcurve,2);
+	config_set_float_a(cg,"fcalgibpha", bsd->fcalgibpha,2);
+	config_set_float_a(cg,"fcalgibmag", bsd->fcalgibmag,2);
 }
 
-void sstrcpy(char* dest, const char *src) {
+void sstrcpy(char** dest, const char *src) {
 	uint32_t len = strlen(src)+1;
-	if (dest == NULL) {
-		dest = (char*)malloc(len);
-	} else {
-		dest = (char*)realloc(dest, len);
-	}
-	strcpy(dest,src);
+	assert(dest!=NULL);
+
+	char* ptr = (char*)realloc(*dest, len); //acts like malloc if dest==NULL
+	assert(ptr!=NULL);
+	*dest = ptr;
+
+	strcpy(*dest,src);
 }
 
-const char* config_read_str(const char *tag, const char *value, const char *def_str) {
+config_entry* find_config_entry(config_group *cg, const char *tag) {
 	uint16_t i = 0;
-	for (i=0;i<used_entries;++i) {
-		if ( strcmp(config_values[i].tag, tag) == 0 ) {
-			return config_values[i].str;
+	for (i=0;i < cg->used_entries;++i) {
+		if ( strcmp(cg->config_entries[i].tag, tag) == 0 ) {
+			return cg->config_entries+i;
 		}
 	}
-	assert(used_entries<MAX_CONFIG_ENTRIES);
+	return NULL;
+}
+
+const char* config_read_str(config_group *cg, const char *tag, const char *def) {
+	config_entry *cv = find_config_entry(cg, tag);
 
-	i = used_entries++;
-	sstrcpy(config_values[i].tag, tag);
-	sstrcpy(config_values[i].str, def_str);
+	if (cv != NULL) return cv->data;
 
-	return config_values[i].str;
+	return config_set_str(cg,tag,def);
 }
 
-uint32_t config_read_uint32(const char *tag, const uint32_t value, const uint32_t def) {
-	uint16_t i = 0;
-	for (i=0;i<used_entries;++i) {
-		if ( strcmp(config_values[i].tag, tag) == 0 ) {
-			return config_values[i].numeric.i;
-		}
-	}
-	assert(used_entries<MAX_CONFIG_ENTRIES);
+uint32_t config_read_uint32(config_group *cg, const char *tag, const uint32_t def) {
+	config_entry *cv = find_config_entry(cg, tag);
 
-	i = used_entries++;
-	sstrcpy(config_values[i].tag, tag);
-	config_values[i].numeric.i = def;
+	if (cv != NULL) return cv->numeric.i;
 
-	return config_values[i].numeric.i;
+	return config_set_uint32(cg, tag, def);
 }
 
-FLT config_read_float(const char *tag, const FLT value, const FLT def) {
-	uint16_t i = 0;
-	for (i=0;i<used_entries;++i) {
-		if ( strcmp(config_values[i].tag, tag) == 0 ) {
-			return config_values[i].numeric.f;
-		}
+FLT config_read_float(config_group *cg, const char *tag, const FLT def) {
+	config_entry *cv = find_config_entry(cg, tag);
+
+	if (cv != NULL) return cv->numeric.f;
+
+	return config_set_float(cg, tag, def);
+}
+
+uint16_t config_read_float_array(config_group *cg, const char *tag, FLT** values, const FLT* def, uint16_t count) {
+	config_entry *cv = find_config_entry(cg, tag);
+
+	if (cv != NULL) {
+		*values = (FLT*)cv->data;
+		return cv->elements;
 	}
-	assert(used_entries<MAX_CONFIG_ENTRIES);
 
-	i = used_entries++;
-	sstrcpy(config_values[i].tag, tag);
-	config_values[i].numeric.f = def;
+	if (def == NULL) return 0;
 
-	return config_values[i].numeric.f;
+	config_set_float_a(cg, tag, def, count);
+	*values = def;
+	return count;
 }
 
-const char* config_set_str(const char *tag, const char* value) {
-	uint16_t i = 0;
+config_entry* next_unused_entry(config_group *cg) {
+	config_entry *cv = NULL;
+//	assert(cg->used_entries < cg->max_entries);
+
+	if (cg->used_entries >= cg->max_entries) resize_config_group(cg, cg->max_entries + 10);
+
+	cv = cg->config_entries + cg->used_entries;
 
-	assert(used_entries<MAX_CONFIG_ENTRIES);
+	cg->used_entries++;
+	return cv;
+}
+
+const char* config_set_str(config_group *cg, const char *tag, const char* value) {
+	config_entry *cv = find_config_entry(cg, tag);
+	if (cv == NULL) cv = next_unused_entry(cg);
 
-	i = used_entries++;
-	sstrcpy(config_values[i].tag, tag);
-	sstrcpy(config_values[i].str, value);
+	sstrcpy(&(cv->tag), tag);
+	sstrcpy(&(cv->data), value);
+	cv->type = CONFIG_STRING;
 
 	return value;
 }
 
-const uint32_t config_set_uint32(const char *tag, const uint32_t value) {
-	uint16_t i = 0;
+const uint32_t config_set_uint32(config_group *cg, const char *tag, const uint32_t value) {
+	config_entry *cv = find_config_entry(cg, tag);
+	if (cv == NULL) cv = next_unused_entry(cg);
 
-	assert(used_entries<MAX_CONFIG_ENTRIES);
+	sstrcpy(&(cv->tag), tag);
+	cv->numeric.i = value;
+	cv->type = CONFIG_UINT32;
 
-	i = used_entries++;
-	sstrcpy(config_values[i].tag, tag);
-	config_values[i].numeric.i = value;
+	return value;
+}
+
+const FLT config_set_float(config_group *cg, const char *tag, const FLT value) {
+	config_entry *cv = find_config_entry(cg, tag);
+	if (cv == NULL) cv = next_unused_entry(cg);
+
+	sstrcpy(&(cv->tag), tag);
+	cv->numeric.f = value;
+	cv->type = CONFIG_FLOAT;
 
 	return value;
 }
 
-const FLT config_set_float(const char *tag, const FLT value) {
+const FLT* config_set_float_a(config_group *cg, const char *tag, const FLT* values, uint8_t count) {
+	config_entry *cv = find_config_entry(cg, tag);
+	if (cv == NULL) cv = next_unused_entry(cg);
+
+	sstrcpy(&(cv->tag), tag);
+
+	char* ptr = (char*)realloc(cv->data, sizeof(FLT)*count);
+	assert(ptr!=NULL);
+	cv->data = ptr;
+
+	printf("float array\n");
+
+	memcpy(cv->data,values,sizeof(FLT)*count);
+	cv->type = CONFIG_FLOAT_ARRAY;
+	cv->elements = count;
+
+	return values;
+}
+
+void _json_write_float_array(FILE* f, const char* tag, FLT* v, uint8_t count) {
+	#ifdef USE_DOUBLE
+		json_write_double_array(f,tag,v,count);
+	#else
+		json_write_float_array(f,tag,v,count);
+	#endif
+}
+
+void write_config_group(FILE* f, config_group *cg, char *tag) {
 	uint16_t i = 0;
 
-	assert(used_entries<MAX_CONFIG_ENTRIES);
+	if (tag != NULL) {
+		fprintf(f, "\"%s\":{\n", tag);
+	}
 
-	i = used_entries++;
-	sstrcpy(config_values[i].tag, tag);
-	config_values[i].numeric.f = value;
+	for (i=0;i < cg->used_entries;++i) {
+		if (cg->config_entries[i].type == CONFIG_FLOAT) {
+			json_write_float(f, cg->config_entries[i].tag, cg->config_entries[i].numeric.f);
+		} else if (cg->config_entries[i].type == CONFIG_UINT32) {
+			json_write_uint32(f, cg->config_entries[i].tag, cg->config_entries[i].numeric.i);
+		} else if (cg->config_entries[i].type == CONFIG_STRING) {
+			json_write_str(f, cg->config_entries[i].tag, cg->config_entries[i].data);
+		} else if (cg->config_entries[i].type == CONFIG_FLOAT_ARRAY) {
+			_json_write_float_array(f, cg->config_entries[i].tag, (FLT*)cg->config_entries[i].data, cg->config_entries[i].elements);
+		}
+		if ((i+1) < cg->used_entries) fprintf(f,",");
+		fprintf(f,"\n");
+	};
 
-	return value;
+	if (tag != NULL) {
+		fprintf(f,"}\n");
+	}
 }
+
 void config_save(const char* path) {
 	uint16_t i = 0;
 
 	FILE* f = fopen(path, "w");
 
-	for (i=0;i<=used_entries;++i) {
-		if (config_values[i].type == CONFIG_FLOAT) {
-			fprintf(f, "\"%s\":\"%F\"\n", config_values[i].tag, config_values[i].numeric.f);
-		} else if (config_values[i].type == CONFIG_UINT32) {
-			fprintf(f, "\"%s\":\"%d\"\n", config_values[i].tag, config_values[i].numeric.i);
-		} else if (config_values[i].type == CONFIG_STRING) {
-			fprintf(f, "\"%s\":\"%s\"\n", config_values[i].tag, config_values[i].str);
-		}
-	};
+	write_config_group(f,&global_config_values, NULL);
+	write_config_group(f,lh_config, "lighthouse0");
+	write_config_group(f,lh_config+1, "lighthouse1");
 
 	fclose(f);
 }
 
+void print_json_value(char* tag, char** values, uint16_t count) {
+	uint16_t i = 0;
+	for (i=0;i<count; ++i) {
+		printf("%s:%s \n", tag, values[i]);
+	}
+}
+
+config_group* cg_stack[10]; //handle 10 nested objects deep
+uint8_t cg_stack_head = 0;
+
+void handle_config_group(char* tag) {
+	cg_stack_head++;
+	if (strcmp("lighthouse0",tag) == 0) {
+		cg_stack[cg_stack_head] = lh_config;
+	} else if (strcmp("lighthouse1",tag) == 0) {
+		cg_stack[cg_stack_head] = lh_config+1;
+	} else {
+		cg_stack[cg_stack_head] = &global_config_values;
+	}
+}
+
+void pop_config_group() {
+	cg_stack_head--;
+}
+
+
+int parse_floats(char* tag, char** values, uint16_t count) {
+	uint16_t i = 0;
+	FLT f[count];
+	char* end = NULL;
+	config_group* cg = cg_stack[cg_stack_head];
+
+	for(i=0;i<count;++i) {
+
+		#ifdef USE_DOUBLE
+		f[i] = strtod(values[i], &end);
+		#else
+		f[i] = strtof(values[i], &end);
+		#endif
+
+//		if (values[i] == end) return 0; //not a float
+		if (*end != '\0') return 0; //not an integer
+	}
+
+	if (count>1) {
+		config_set_float_a(cg, tag, f, count);
+	}
+	else {
+		config_set_float(cg, tag, f[0]);
+	}
+
+	return 1;
+}
+
+int parse_uint32(char* tag, char** values, uint16_t count) {
+	uint16_t i = 0;
+	uint32_t l[count];
+	char* end = NULL;
+	config_group* cg = cg_stack[cg_stack_head];
+
+/*
+	//look for non numeric values
+	for(end=values[0];*end!='\0';++end) {
+		if ((*end<48) || (*end>57)) return 0;
+	}
+
+	end=NULL;
+*/
+	for(i=0;i<count;++i) {
+		l[i] = strtol(values[i], &end, 10);
+//		if (values[i] == end) return 0; //not an integer
+		if (*end != '\0') return 0; //not an integer
+	}
+
+//	if (count>1)
+//		config_set_uint32_array(cg, tag, f, count);
+//	else
+		config_set_uint32(cg, tag, l[0]);
+
+	return 1;
+}
+
+void handle_tag_value(char* tag, char** values, uint16_t count) {
+	print_json_value(tag,values,count);
+	config_group* cg = cg_stack[cg_stack_head];
+
+	if (parse_uint32(tag,values,count) > 0) return; //parse integers first, stricter rules
+
+	if (parse_floats(tag,values,count) > 0) return;
+
+	//should probably also handle string arrays
+	config_set_str(cg,tag,values[0]);
+//	else if (count>1) config_set_str
+}
+
+void config_read(const char* path) {
+	json_begin_object = handle_config_group;
+	json_end_object = pop_config_group;
+	json_tag_value = handle_tag_value;
+
+	cg_stack[0] = &global_config_values;
+
+	json_load_file(path);
+
+	json_begin_object = NULL;
+	json_end_object = NULL;
+	json_tag_value = NULL;
+}
+
diff --git a/src/survive_config.h b/src/survive_config.h
index 24762cd..cd16c76 100644
--- a/src/survive_config.h
+++ b/src/survive_config.h
@@ -10,14 +10,11 @@ typedef enum {
 	CONFIG_UNKNOWN = 0,
 	CONFIG_FLOAT = 1,
 	CONFIG_UINT32 = 2,
-	CONFIG_STRING = 3
+	CONFIG_STRING = 3,
+	CONFIG_FLOAT_ARRAY = 4,
 } cval_type;
-/*
-typedef union {
-		uint32_t i;
-		FLT f;
-	} Numeric;
-*/
+
+
 typedef struct {
 	char *tag;
 	cval_type type;
@@ -25,21 +22,34 @@ typedef struct {
 		uint32_t i;
 		FLT f;
 	} numeric;
-	char *str;
-} config_val;
+	char *data;
+	uint32_t elements;
+} config_entry;
+
+typedef struct {
+	config_entry *config_entries;
+	uint16_t	used_entries;
+	uint16_t	max_entries;
+} config_group;
+
+extern config_group global_config_values;
+extern config_group lh_config[2]; //lighthouse configs
 
 
-void config_open(const char* path, const char* mode);
-void config_close();
-void config_write_lighthouse(struct BaseStationData* bsd, uint8_t length);
+void config_init();
+//void config_open(const char* path, const char* mode);
+void config_read(const char* path);
+//void config_write_lighthouse(struct BaseStationData* bsd, uint8_t length);
+void config_set_lighthouse(BaseStationData* bsd, uint8_t idx);
 
 void config_save(const char* path);
-const FLT config_set_float(const char *tag, const FLT value);
-const uint32_t config_set_uint32(const char *tag, const uint32_t value);
-const char* config_set_str(const char *tag, const char* value);
-FLT config_read_float(const char *tag, const FLT value, const FLT def);
+const FLT config_set_float(config_group *cg, const char *tag, const FLT value);
+const uint32_t config_set_uint32(config_group *cg, const char *tag, const uint32_t value);
+const char* config_set_str(config_group *cg, const char *tag, const char* value);
 
-uint32_t config_read_uint32(const char *tag, const uint32_t value, const uint32_t def);
-const char* config_read_str(const char *tag, const char *value, const char *def_str);
+FLT config_read_float(config_group *cg, const char *tag, const FLT def);
+uint16_t config_read_float_array(config_group *cg, const char *tag, FLT** values, const FLT* def, uint16_t count);
+uint32_t config_read_uint32(config_group *cg, const char *tag, const uint32_t def);
+const char* config_read_str(config_group *cg, const char *tag, const char *def);
 
-#endif
\ No newline at end of file
+#endif
diff --git a/src/survive_data.c b/src/survive_data.c
index e55b640..63cc5c2 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -6,12 +6,19 @@
 #include <string.h>
 
 //This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode.
-void handle_lightcap( struct SurviveObject * so, struct LightcapElement * le )
+void handle_lightcap( SurviveObject * so, LightcapElement * le )
 {
-	struct SurviveContext * ctx = so->ctx;
+	SurviveContext * ctx = so->ctx;
 	//int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time;
 
-//	printf( "%s %d %d %d %d %d\n", so->codename, le->sensor_id, le->type, le->length, le->timestamp, le->timestamp-so->tsl );
+	//if( so->codename[0] != 'H' )
+	//printf( "*** %s %d %d %d %d %d\n", so->codename, le->sensor_id, le->type, le->length, le->timestamp, le->timestamp-so->tsl );
+
+	if( le->sensor_id > SENSORS_PER_OBJECT )
+	{
+		return;
+	}
+
 
 	so->tsl = le->timestamp;
 	if( le->length < 20 ) return;  ///Assuming 20 is an okay value for here.
diff --git a/src/survive_driverman.c b/src/survive_driverman.c
index 8cdfb71..d694e64 100644
--- a/src/survive_driverman.c
+++ b/src/survive_driverman.c
@@ -3,7 +3,7 @@
 // See notice in survive_driverman.h
 //
 
-#include "survive_driverman.h"
+#include "survive_internal.h"
 #include <string.h>
 #include <stdio.h>
 
diff --git a/src/survive_driverman.h b/src/survive_driverman.h
deleted file mode 100644
index 5e13caf..0000000
--- a/src/survive_driverman.h
+++ /dev/null
@@ -1,38 +0,0 @@
-// (C) 2017 <>< C. N. Lohr, Under MIT/x11 License.
-//
-// This file is intended to be used for self-registering functions.  By using
-// this it means that you do not need to have complicated switch statements or
-// #defines for dfferent inclusion of drivers/other code.  You can simply
-// register your function and it will be put into a list.
-//
-//
-
-#ifndef SURVIVE_DRIVERMAN_H
-#define SURVIVE_DRIVERMAN_H
-
-//Driver registration
-#define MAX_DRIVERS 32
-
-void   RegisterDriver( const char * name, void * data );
-void * GetDriver( const char * name );
-const char * GetDriverNameMatching( const char * prefix, int place );
-void   ListDrivers();
-
-#define REGISTER_LINKTIME( func ) \
-	void __attribute__((constructor)) Register##func() { RegisterDriver( #func, &func ); }
-
-
-//
-// Specific types of drivers.
-//
-
-struct SurviveContext;
-
-//Device drivers (prefix your drivers with "DriverReg") i.e.
-//		REGISTER_LINKTIME( DriverRegHTCVive );
-typedef int (*DeviceDriver)( struct SurviveContext * ctx );
-
-//more driver types here? i.e. posefinders, etc.
-
-#endif
-
diff --git a/src/survive_internal.h b/src/survive_internal.h
index 5962623..392104a 100644
--- a/src/survive_internal.h
+++ b/src/survive_internal.h
@@ -1,15 +1,5 @@
-//<>< (C) 2016 C. N. Lohr, MOSTLY Under MIT/x11 License.
+//<>< (C) 2016-2017 C. N. Lohr, MOSTLY Under MIT/x11 License.
 //
-//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.
-
 
 #ifndef _SURVIVE_INTERNAL_H
 #define _SURVIVE_INTERNAL_H
@@ -17,88 +7,16 @@
 #include <stdlib.h>
 #include <stdio.h>
 #include <stdint.h>
-#include "survive_driverman.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 ); }
-
-//XXX TODO This one needs to be rewritten.
-#define SV_KILL()		exit(0)
-
-#define INTBUFFSIZE			64
-#define SENSORS_PER_OBJECT	32
-
-struct SurviveContext;
-struct SurviveUSBInterface;
-
-typedef int (*DeviceDriverCb)( struct SurviveContext * ctx, void * driver );
-typedef int (*DeviceDriverMagicCb)( struct SurviveContext * ctx, void * driver, int magic_code, void * data, int datalen );
-
-
-//This is defined in survive.h
-struct SurviveObject;
-struct SurviveCalData;
-
-struct BaseStationData
-{
-	uint8_t PositionSet:1;
-	FLT Position[3];
-	FLT Quaternion[4];
-
-	uint8_t OOTXSet:1;
-	uint32_t BaseStationID;
-	FLT fcalphase[2];
-	FLT fcaltilt[2];
-	FLT fcalcurve[2];
-	FLT fcalgibpha[2];
-	FLT fcalgibmag[2];
-};
-
-struct SurviveContext
-{
-	text_feedback_func faultfunction;
-	text_feedback_func notefunction;
-	light_process_func lightproc;
-	imu_process_func imuproc;
-	angle_process_func angleproc;
-
-	//Calibration data:
-	struct BaseStationData bsd[NUM_LIGHTHOUSES];
-
-	struct SurviveCalData * calptr; //If and only if the calibration subsystem is attached.
-
-	struct SurviveObject ** objs;
-	int objs_ct;
-
-	void ** drivers;
-	DeviceDriverCb * driverpolls;
-	DeviceDriverCb * drivercloses;
-	DeviceDriverMagicCb * drivermagics;
-	int driver_ct;
-};
-
-int survive_add_object( struct SurviveContext * ctx, struct SurviveObject * obj );
-
-void survive_add_driver( struct SurviveContext * ctx, void * payload, DeviceDriverCb poll, DeviceDriverCb close, DeviceDriverMagicCb magic );
-
-//For lightcap, etc.  Don't change this structure at all.  Regular vive is dependent on it being exactly as-is.
-struct LightcapElement
-{
-	uint8_t sensor_id;
-	uint8_t type;
-	uint16_t length;
-	uint32_t timestamp;
-} __attribute__((packed));
-
-//This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode.
-void handle_lightcap( struct SurviveObject * so, struct LightcapElement * le );
-
 
-//Accept Data from backend.
-void survive_data_cb( struct SurviveUSBInterface * si );
+//Driver registration
+#define MAX_DRIVERS 32
 
+void * GetDriver( const char * name );
+const char * GetDriverNameMatching( const char * prefix, int place );
+void   ListDrivers();
 
 #endif
 
diff --git a/src/survive_process.c b/src/survive_process.c
index 75453da..2fea99d 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -6,9 +6,9 @@
 //XXX TODO: Once data is avialble in the context, use the stuff here to handle converting from time codes to
 //proper angles, then from there perform the rest of the solution. 
 
-void survive_default_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length  )
+void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length  )
 {
-	struct SurviveContext * ctx = so->ctx;
+	SurviveContext * ctx = so->ctx;
 	int base_station = acode >> 2;
 	int axis = acode & 1;
 
@@ -25,7 +25,7 @@ void survive_default_light_process( struct SurviveObject * so, int sensor_id, in
 
 	//Need to now do angle correction.
 #if 1
-	struct BaseStationData * bsd = &ctx->bsd[base_station];
+	BaseStationData * bsd = &ctx->bsd[base_station];
 
 	//XXX TODO: This seriously needs to be worked on.  See: https://github.com/cnlohr/libsurvive/issues/18
 	angle += bsd->fcalphase[axis];
@@ -39,20 +39,41 @@ void survive_default_light_process( struct SurviveObject * so, int sensor_id, in
 }
 
 
-void survive_default_angle_process( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle )
+void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle )
 {
-	struct SurviveContext * ctx = so->ctx;
+	SurviveContext * ctx = so->ctx;
 	if( ctx->calptr )
 	{
 		survive_cal_angle( so, sensor_id, acode, timecode, length, angle );
 	}
-
-	//TODO: Writeme!
+	if( so->PoserFn )
+	{
+		PoserDataLight l = {
+			.pt = POSERDATA_LIGHT,
+			.sensor_id = sensor_id,
+			.acode = acode,
+			.timecode = timecode,
+			.length = length,
+			.angle = angle,
+		};
+		so->PoserFn( so, (PoserData *)&l );
+	}
 }	
 
 
-void survive_default_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id )
+void survive_default_imu_process( SurviveObject * so, int mask, FLT * accelgyromag, uint32_t timecode, int id )
 {
-	//TODO: Writeme!
+	if( so->PoserFn )
+	{
+		PoserDataIMU imu = {
+			.pt = POSERDATA_IMU,
+			.datamask = mask,
+			.accel = { accelgyromag[0], accelgyromag[1], accelgyromag[2] },
+			.gyro = {  accelgyromag[3], accelgyromag[4], accelgyromag[5] },
+			.mag = {   accelgyromag[6], accelgyromag[7], accelgyromag[8] },
+			.timecode = timecode,
+		};
+		so->PoserFn( so, (PoserData *)&imu );
+	}
 }
 
diff --git a/src/survive_vive.c b/src/survive_vive.c
index a402153..7da2897 100644
--- a/src/survive_vive.c
+++ b/src/survive_vive.c
@@ -10,26 +10,18 @@
 //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_internal.h"
-#include "survive_driverman.h"
+#include <survive.h>
 #include <jsmn.h>
-
-#include "survive_internal.h"
 #include <libusb-1.0/libusb.h>
 #include <stdio.h>
-#include <unistd.h> //sleep if I ever use it.
+#include <stdlib.h>
+#include <unistd.h>
 #include <errno.h>
 #include <string.h>
+#include <sys/stat.h>
 
 struct SurviveViveData;
 
-//USB Subsystem 
-void survive_usb_close( struct SurviveContext * t );
-int survive_usb_init( struct SurviveViveData * sv, struct SurviveObject * hmd, struct SurviveObject *wm0, struct SurviveObject * wm1 );
-int survive_usb_poll( struct SurviveContext * ctx );
-int survive_get_config( char ** config, struct SurviveViveData * ctx, int devno, int interface, int send_extra_magic );
-
-
 const short vidpids[] = {
 	0x0bb4, 0x2c87, 0, //The main HTC HMD device
 	0x28de, 0x2000, 0, //Valve lighthouse
@@ -59,15 +51,18 @@ const char * devnames[] = {
 #define USB_IF_LIGHTCAP		4
 #define MAX_INTERFACES				5
 
-typedef void (*usb_callback)( struct SurviveUSBInterface * ti );
+typedef struct SurviveUSBInterface SurviveUSBInterface;
+typedef struct SurviveViveData SurviveViveData;
+
+typedef void (*usb_callback)( SurviveUSBInterface * ti );
 
 struct SurviveUSBInterface
 {
-	struct SurviveViveData * sv;
-	struct SurviveContext * ctx;
+	SurviveViveData * sv;
+	SurviveContext * ctx;
 
 	struct libusb_transfer * transfer;
-	struct SurviveObject * assoc_obj;
+	SurviveObject * assoc_obj;
 	int actual_len;
 	uint8_t buffer[INTBUFFSIZE];
 	usb_callback cb;
@@ -77,16 +72,24 @@ struct SurviveUSBInterface
 
 struct SurviveViveData
 {
-	struct SurviveContext * ctx;
+	SurviveContext * ctx;
 
 	//XXX TODO: UN-STATICIFY THIS.
-	struct SurviveUSBInterface uiface[MAX_INTERFACES];
+	SurviveUSBInterface uiface[MAX_INTERFACES];
 	//USB Subsystem
 	struct libusb_context* usbctx;
 	struct libusb_device_handle * udev[MAX_USB_DEVS];
 
 };
 
+void survive_data_cb( SurviveUSBInterface * si );
+
+//USB Subsystem 
+void survive_usb_close( SurviveContext * t );
+int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *wm0, SurviveObject * wm1 );
+int survive_usb_poll( SurviveContext * ctx );
+int survive_get_config( char ** config, SurviveViveData * ctx, int devno, int interface, int send_extra_magic );
+
 
 
 
@@ -114,10 +117,10 @@ static void handle_transfer(struct libusb_transfer* transfer)
 
 
 
-static int AttachInterface( struct SurviveViveData * sv, struct SurviveObject * assocobj, int which_interface_am_i, libusb_device_handle * devh, int endpoint, usb_callback cb, const char * hname )
+static int AttachInterface( SurviveViveData * sv, SurviveObject * assocobj, int which_interface_am_i, libusb_device_handle * devh, int endpoint, usb_callback cb, const char * hname )
 {
-	struct SurviveContext * ctx = sv->ctx;
-	struct SurviveUSBInterface * iface = &sv->uiface[which_interface_am_i];
+	SurviveContext * ctx = sv->ctx;
+	SurviveUSBInterface * iface = &sv->uiface[which_interface_am_i];
 	iface->ctx = ctx;
 	iface->sv = sv;
 	iface->which_interface_am_i = which_interface_am_i;
@@ -592,7 +595,13 @@ 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;
-		w->ctx->imuproc( w, (int16_t *)&readdata[1], (time1<<24)|(time2<<16)|readdata[0], 0 );
+		//XXX XXX BIG TODO!!! Actually recal gyro data.
+		FLT agm[9] = { readdata[1], readdata[2], readdata[3],
+						readdata[4], readdata[5], readdata[6],
+						0,0,0 };
+
+		w->ctx->imuproc( w, 3, agm, (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;
@@ -679,7 +688,7 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata )
 		}
 
 
-		struct LightcapElement les[10];
+		LightcapElement les[10];
 		int lese = 0; //les's end
 
 		//Second, go through all LEDs and extract the lightevent from them. 
@@ -710,7 +719,7 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata )
 				//reverse sorted, but that is to minimize operations.  To read it
 				//in sorted order simply read it back backwards.
 				//Use insertion sort, since we should most of the time, be in order.
-				struct LightcapElement * le = &les[lese++];
+				LightcapElement * le = &les[lese++];
 				le->sensor_id = led;
 				le->type = 0xfe;
 
@@ -724,7 +733,7 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata )
 				int swap = lese-2;
 				while( swap >= 0 && les[swap].timestamp < les[swap+1].timestamp )
 				{
-					struct LightcapElement l;
+					LightcapElement l;
 					memcpy( &l, &les[swap], sizeof( l ) );
 					memcpy( &les[swap], &les[swap+1], sizeof( l ) );
 					memcpy( &les[swap+1], &l, sizeof( l ) );
@@ -743,17 +752,17 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata )
 		return;
 	end:
 		{
-			struct SurviveContext * ctx = w->ctx;
+			SurviveContext * ctx = w->ctx;
 			SV_INFO( "Light decoding fault: %d\n", fault );
 		}
 	}
 }
 
 
-void survive_data_cb( struct SurviveUSBInterface * si )
+void survive_data_cb( SurviveUSBInterface * si )
 {
 	int size = si->actual_len;
-	struct SurviveContext * ctx = si->ctx;
+	SurviveContext * ctx = si->ctx;
 #if 0
 	int i;
 	printf( "%16s: %d: ", si->hname, len );
@@ -766,7 +775,7 @@ void survive_data_cb( struct SurviveUSBInterface * si )
 #endif 
 
 	int iface = si->which_interface_am_i;
-	struct SurviveObject * obj = si->assoc_obj;
+	SurviveObject * obj = si->assoc_obj;
 	uint8_t * readdata = si->buffer;
 
 	int id = POP1;
@@ -807,7 +816,13 @@ void survive_data_cb( struct SurviveUSBInterface * si )
 			if( cd > 0 )
 			{
 				obj->oldcode = code;
-				ctx->imuproc( obj, acceldata, timecode, code );
+
+				//XXX XXX BIG TODO!!! Actually recal gyro data.
+				FLT agm[9] = { acceldata[0], acceldata[1], acceldata[2],
+								acceldata[3], acceldata[4], acceldata[5],
+								0,0,0 };
+
+				ctx->imuproc( obj, 3, agm, timecode, code );
 			}
 		}
 
@@ -817,7 +832,7 @@ void survive_data_cb( struct SurviveUSBInterface * si )
 	case USB_IF_WATCHMAN1:
 	case USB_IF_WATCHMAN2:
 	{
-		struct SurviveObject * w = obj;
+		SurviveObject * w = obj;
 		if( id == 35 )
 		{
 			handle_watchman( w, readdata);
@@ -843,7 +858,7 @@ void survive_data_cb( struct SurviveUSBInterface * si )
 		int i;
 		for( i = 0; i < 7; i++ )
 		{
-			handle_lightcap( obj, (struct LightcapElement*)&readdata[i*8] );
+			handle_lightcap( obj, (LightcapElement*)&readdata[i*8] );
 		}
 		break;
 	}
@@ -874,7 +889,7 @@ static int jsoneq(const char *json, jsmntok_t *tok, const char *s) {
 }
 
 
-static int ParsePoints( struct SurviveContext * ctx, struct 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;
@@ -913,9 +928,9 @@ static int ParsePoints( struct SurviveContext * ctx, struct SurviveObject * so,
 	return 0;
 }
 
-static int LoadConfig( struct SurviveViveData * sv, struct SurviveObject * so, int devno, int iface, int extra_magic )
+static int LoadConfig( SurviveViveData * sv, SurviveObject * so, int devno, int iface, int extra_magic )
 {
-	struct SurviveContext * ctx = sv->ctx;
+	SurviveContext * ctx = sv->ctx;
 	char * ct0conf = 0;
 	int len = survive_get_config( &ct0conf, sv, devno, iface, extra_magic );
 
@@ -976,25 +991,46 @@ static int LoadConfig( struct SurviveViveData * sv, struct SurviveObject * so, i
 		//TODO: Cleanup any remaining USB stuff.
 		return 1;
 	}
+
+	char fname[20];
+	mkdir( "calinfo", 0755 );
+
+	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;
 }
 
 
-int survive_vive_close( struct SurviveContext * ctx, void * driver )
+int survive_vive_close( SurviveContext * ctx, void * driver )
 {
-	struct SurviveViveData * sv = driver;
+	SurviveViveData * sv = driver;
  
 	survive_vive_usb_close( sv );
 }
 
 
-int DriverRegHTCVive( struct SurviveContext * ctx )
+int DriverRegHTCVive( SurviveContext * ctx )
 {
 	int i, r;
-	struct SurviveObject * hmd = calloc( 1, sizeof( struct SurviveObject ) );
-	struct SurviveObject * wm0 = calloc( 1, sizeof( struct SurviveObject ) );
-	struct SurviveObject * wm1 = calloc( 1, sizeof( struct SurviveObject ) );
-	struct SurviveViveData * sv = calloc( 1, sizeof( struct SurviveViveData ) );
+	SurviveObject * hmd = calloc( 1, sizeof( SurviveObject ) );
+	SurviveObject * wm0 = calloc( 1, sizeof( SurviveObject ) );
+	SurviveObject * wm1 = calloc( 1, sizeof( SurviveObject ) );
+	SurviveViveData * sv = calloc( 1, sizeof( SurviveViveData ) );
 
 	sv->ctx = ctx;