More cleanup & finishing genericization of calibrator

5 files changed, 31 insertions, 23 deletions

diff --git a/src/survive_cal.c b/src/survive_cal.c
index 22a8eff..6c153b4 100755
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -220,10 +220,13 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int
 		else if( acode < -4 ) break;
 		int lh = (-acode) - 3;
 
-		if( strcmp( so->codename, "WM0" ) == 0 )
-			sensor_id += 32;
-		if( strcmp( so->codename, "WM1" ) == 0 )
-			sensor_id += 64;
+		for (int i=0; i < min(MAX_DEVICES_TO_CAL, cd->numPoseObjects); i++)
+		{
+			if( strcmp( so->codename, cd->poseobjects[i]->codename ) == 0 )
+			{
+				sensor_id += i*32;
+			}
+		}
 
 		cd->all_sync_times[sensor_id][lh][cd->all_sync_counts[sensor_id][lh]++] = length;
 		break;
@@ -233,7 +236,7 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int
 	
 }
 
-void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle )
+void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh )
 {
 	struct SurviveContext * ctx = so->ctx;
 	struct SurviveCalData * cd = ctx->calptr;
@@ -241,14 +244,18 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin
 	if( !cd ) return;
 
 	int sensid = sensor_id;
-	if( strcmp( so->codename, "WM0" ) == 0 )
-		sensid += 32;
-	if( strcmp( so->codename, "WM1" ) == 0 )
-		sensid += 64;
+
+	for (int i=0; i < min(MAX_DEVICES_TO_CAL, cd->numPoseObjects); i++)
+	{
+		if( strcmp( so->codename, cd->poseobjects[i]->codename ) == 0 )
+		{
+			sensid += i*32;
+		}
+	}
 
 	if( sensid >= MAX_SENSORS_TO_CAL || sensid < 0 ) return;
 
-	int lighthouse = acode>>2;
+	int lighthouse = lh;
 	int axis = acode & 1;
 
 	switch( cd->stage )
@@ -292,7 +299,8 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin
 			int min_peaks = PTS_BEFORE_COMMON;
 			int i, j, k;
 			cd->found_common = 1;
-			for( i = 0; i < MAX_SENSORS_TO_CAL/SENSORS_PER_OBJECT; i++ )
+			for( i = 0; i < cd->numPoseObjects; i++ )
+			//for( i = 0; i < MAX_SENSORS_TO_CAL/SENSORS_PER_OBJECT; i++ )
 			for( j = 0; j < NUM_LIGHTHOUSES; j++ )
 			{
 				int sensors_visible = 0;
diff --git a/src/survive_cal.h b/src/survive_cal.h
index 8f4e4de..ae644d1 100644
--- a/src/survive_cal.h
+++ b/src/survive_cal.h
@@ -30,9 +30,11 @@ int survive_cal_get_status( SurviveContext * ctx, char * description, int descri
 
 //Called from survive_default_light_process
 void survive_cal_light( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lighthouse);
-void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle );
+void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh );
 
-#define MAX_SENSORS_TO_CAL 96
+#define MAX_SENSORS_PER_DEVICE 32
+#define MAX_DEVICES_TO_CAL 3
+#define MAX_SENSORS_TO_CAL (MAX_SENSORS_PER_DEVICE * MAX_DEVICES_TO_CAL)
 
 #define MIN_PTS_BEFORE_CAL 24
 
diff --git a/src/survive_data.c b/src/survive_data.c
index 4e2479a..9447104 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -25,7 +25,7 @@ typedef struct
 
 typedef struct
 {
-	float acode_offset;
+	double acode_offset;
 } lightcap2_global_data;
 
 typedef struct
@@ -40,11 +40,11 @@ typedef struct
 
 int handle_lightcap2_getAcodeFromSyncPulse(SurviveObject * so, int pulseLen)
 {
-	float oldOffset = ((lightcap2_data*)so->disambiguator_data)->global.acode_offset;
+	double oldOffset = ((lightcap2_data*)so->disambiguator_data)->global.acode_offset;
 
 	int modifiedPulseLen = pulseLen - (int)oldOffset;
 
-	float newOffset = (((pulseLen) + 250) % 500) - 250;
+	double newOffset = (((pulseLen) + 250) % 500) - 250;
 
 	((lightcap2_data*)so->disambiguator_data)->global.acode_offset = oldOffset * 0.9 + newOffset * 0.1;
 
diff --git a/src/survive_process.c b/src/survive_process.c
index d4604d8..b58b344 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -37,16 +37,16 @@ void survive_default_light_process( SurviveObject * so, int sensor_id, int acode
 #endif
 
 	FLT length_sec = length / (FLT)so->timebase_hz;
-	ctx->angleproc( so, sensor_id, acode, timecode, length_sec, angle );
+	ctx->angleproc( so, sensor_id, acode, timecode, length_sec, angle, lh);
 }
 
 
-void survive_default_angle_process( 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, uint32_t lh)
 {
 	SurviveContext * ctx = so->ctx;
 	if( ctx->calptr )
 	{
-		survive_cal_angle( so, sensor_id, acode, timecode, length, angle );
+		survive_cal_angle( so, sensor_id, acode, timecode, length, angle, lh );
 	}
 	if( so->PoserFn )
 	{
diff --git a/src/survive_vive.c b/src/survive_vive.c
index 55e949a..a5c731d 100755
--- a/src/survive_vive.c
+++ b/src/survive_vive.c
@@ -834,7 +834,6 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata )
 	qty-=2;
 	int propset = 0;
 	int doimu = 0;
-	int i;
 
 	if( (type & 0xf0) == 0xf0 )
 	{
@@ -911,10 +910,9 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata )
 		*readdata = type; //Put 'type' back on stack.
 		uint8_t * mptr = readdata + qty-3-1; //-3 for timecode, -1 to 
 
-//#define DEBUG_WATCHMAN
 #ifdef DEBUG_WATCHMAN
 		printf( "_%s ", w->codename);
-		for( i = 0; i < qty; i++ )
+		for(int i = 0; i < qty; i++ )
 		{
 			printf( "%02x ", readdata[i] );
 		}
@@ -1198,7 +1196,7 @@ void survive_data_cb( SurviveUSBInterface * si )
 			unsigned short *length = (unsigned short *)(&(readdata[2]));
 			unsigned long *time = (unsigned long *)(&(readdata[4]));
 			LightcapElement le;
-			le.sensor_id = POP2;
+			le.sensor_id = (uint8_t)POP2;
 			le.length = POP2;
 			le.timestamp = POP4;
 			if( le.sensor_id == 0xff ) break;