Move to having an angle callback.

6 files changed, 97 insertions, 29 deletions

diff --git a/src/survive.c b/src/survive.c
index ed18da2..aab2ae6 100644
--- a/src/survive.c
+++ b/src/survive.c
@@ -30,7 +30,7 @@ static void survivenote( struct SurviveContext * ctx, const char * fault )
 	fprintf( stderr, "Info: %s\n", fault );
 }
 
-static int ParsePoints( struct SurviveContext * ctx, struct SurviveObject * so, char * ct0conf, SV_FLOAT ** floats_out, jsmntok_t * t, int i )
+static int ParsePoints( struct SurviveContext * ctx, struct SurviveObject * so, char * ct0conf, FLT ** floats_out, jsmntok_t * t, int i )
 {
 	int k;
 	int pts = t[i+1].size;
@@ -43,7 +43,7 @@ static int ParsePoints( struct SurviveContext * ctx, struct SurviveObject * so,
 	{
 		tk = &t[i+2+k*4];
 
-		float vals[3];
+		FLT vals[3];
 		int m;
 		for( m = 0; m < 3; m++ )
 		{
@@ -60,7 +60,7 @@ static int ParsePoints( struct SurviveContext * ctx, struct SurviveObject * so,
 
 			memcpy( ctt, ct0conf + tk->start, elemlen );
 			ctt[elemlen] = 0;
-			float f = atof( ctt );
+			FLT f = atof( ctt );
 			int id = so->nr_locations*3+m;
 			(*floats_out)[id] = f;
 		}
@@ -144,6 +144,7 @@ struct SurviveContext * survive_init()
 
 	ctx->lightproc = survive_default_light_process;
 	ctx->imuproc = survive_default_imu_process;
+	ctx->angleproc = survive_default_angle_process;
 
 	ctx->headset.ctx = ctx;
 	memcpy( ctx->headset.codename, "HMD", 4 );
@@ -164,6 +165,18 @@ struct SurviveContext * survive_init()
 	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;
+	ctx->headset.pulselength_min_sync = ctx->watchman[0].pulselength_min_sync = ctx->watchman[1].pulselength_min_sync = 2200;
+	ctx->headset.pulse_in_clear_time = ctx->watchman[0].pulse_in_clear_time = ctx->watchman[1].pulse_in_clear_time = 35000;
+	ctx->headset.pulse_max_for_sweep = ctx->watchman[0].pulse_max_for_sweep = ctx->watchman[1].pulse_max_for_sweep = 1800;
+
+	ctx->headset.pulse_synctime_offset = ctx->watchman[0].pulse_synctime_offset = ctx->watchman[1].pulse_synctime_offset = 20000;
+	ctx->headset.pulse_synctime_slack = ctx->watchman[0].pulse_synctime_slack = ctx->watchman[1].pulse_synctime_slack = 5000;
+
+	ctx->headset.timecenter_ticks     = ctx->headset.timebase_hz / 240;
+	ctx->watchman[0].timecenter_ticks = ctx->watchman[0].timebase_hz / 240;
+	ctx->watchman[1].timecenter_ticks = ctx->watchman[1].timebase_hz / 240;
 /*
 	int i;
 	int locs = ctx->headset.nr_locations;
@@ -195,7 +208,7 @@ fail_gracefully:
 	return 0;
 }
 
-void survive_install_info_fn( struct SurviveContext * ctx,  text_feedback_fnptr fbp )
+void survive_install_info_fn( struct SurviveContext * ctx,  text_feedback_func fbp )
 {
 	if( fbp )
 		ctx->notefunction = fbp;
@@ -203,7 +216,7 @@ void survive_install_info_fn( struct SurviveContext * ctx,  text_feedback_fnptr
 		ctx->notefunction = survivenote;
 }
 
-void survive_install_error_fn( struct SurviveContext * ctx,  text_feedback_fnptr fbp )
+void survive_install_error_fn( struct SurviveContext * ctx,  text_feedback_func fbp )
 {
 	if( fbp )
 		ctx->faultfunction = fbp;
@@ -227,6 +240,17 @@ void survive_install_imu_fn( struct SurviveContext * ctx,  imu_process_func fbp
 		ctx->imuproc = survive_default_imu_process;
 }
 
+
+void survive_install_angle_fn( struct SurviveContext * ctx,  angle_process_func fbp )
+{
+	if( fbp )
+		ctx->angleproc = fbp;
+	else
+		ctx->angleproc = survive_default_angle_process;
+}
+
+
+
 void survive_close( struct SurviveContext * ctx )
 {
 	survive_usb_close( ctx );
diff --git a/src/survive_cal.c b/src/survive_cal.c
index cdc7783..6bb8173 100644
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -5,6 +5,7 @@
 
 #include "survive_cal.h"
 #include "survive_internal.h"
+#include <math.h>
 
 void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet)
 {
@@ -23,8 +24,8 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet)
 	b->BaseStationID = v6.id;
 	b->fcalphase[0] = v6.fcal_0_phase;
 	b->fcalphase[1] = v6.fcal_1_phase;
-	b->fcaltilt[0] = v6.fcal_0_tilt;
-	b->fcaltilt[1] = v6.fcal_1_tilt;
+	b->fcaltilt[0] = tan(v6.fcal_0_tilt);
+	b->fcaltilt[1] = tan(v6.fcal_1_tilt);  //XXX??? Is this right? See https://github.com/cnlohr/libsurvive/issues/18
 	b->fcalcurve[0] = v6.fcal_0_curve;
 	b->fcalcurve[1] = v6.fcal_1_curve;
 	b->fcalgibpha[0] = v6.fcal_0_gibphase;
@@ -76,7 +77,6 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int
 			if( lhid < NUM_LIGHTHOUSES && so->codename[0] == 'H' )
 			{
 				uint8_t dbit = (acode & 2)>>1;
-				//printf( "%s %d %d %d\n", so->codename, lhid, acode, dbit );
 				ootx_pump_bit( &cd->ootx_decoders[lhid], dbit );
 			}
 			int i;
@@ -85,9 +85,29 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int
 			if( i == NUM_LIGHTHOUSES ) cd->stage = 2;  //If all lighthouses have their OOTX set, move on.
 		}
 		break;
+	case 2:      //Taking in angle data.
+		break;
+	}
+}
+
+void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle )
+{
+	struct SurviveContext * ctx = so->ctx;
+	struct SurviveCalData * cd = ctx->calptr;
+
+	if( !cd ) return;
 
+	switch( cd->stage )
+	{
+	default:
+	case 1:	//Collecting OOTX data. (Don't do anything here, yet.)
+	case 0: //Default, inactive.
+		break;
 	case 2:
+		//if( sensor_id == 0 && so->codename[0] == 'H' ) printf( "%d %f %f\n", acode, length, angle );
 		break;
 	}
 }
 
+
+
diff --git a/src/survive_cal.h b/src/survive_cal.h
index a74bf95..6653a84 100644
--- a/src/survive_cal.h
+++ b/src/survive_cal.h
@@ -28,6 +28,7 @@ int survive_cal_get_status( struct SurviveContext * ctx, char * description, int
 
 //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 );
 
 struct SurviveCalData
 {
diff --git a/src/survive_data.c b/src/survive_data.c
index c557951..ad834cf 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -37,36 +37,34 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
 //	printf( "%s %d %d %d %d %d\n", so->codename, le->sensor_id, le->type, le->length, le->timestamp, le->timestamp-so->tsl );
 
 	so->tsl = le->timestamp;
-	if( le->length < 20 ) return;
+	if( le->length < 20 ) return;  ///Assuming 20 is an okay value for here.
 
 	//The sync pulse finder is taking Charles's old disambiguator code and mixing it with a more linear
 	//version of Julian Picht's disambiguator, available in 488c5e9.  Removed afterwards into this
 	//unified driver.
-
-
 	int ssn = so->sync_set_number;
 	if( ssn < 0 ) ssn = 0;
 	int last_sync_time  =  so->last_time  [ssn];
 	int last_sync_length = so->last_length[ssn];
 	int32_t delta = le->timestamp - last_sync_time;  //Handle time wrapping (be sure to be int32)
 
-	if( delta < -500000 || delta > 500000 )
+	if( delta < -so->pulsedist_max_ticks || delta > so->pulsedist_max_ticks )
 	{
 		//Reset pulse, etc.
 		so->sync_set_number = -1;
-		delta = 500000;
+		delta = so->pulsedist_max_ticks;
 	}
 
 
-	if( le->length > 2200 ) //Pulse longer indicates a sync pulse.
+	if( le->length > so->pulselength_min_sync ) //Pulse longer indicates a sync pulse.
 	{
-		int is_new_pulse = delta > 1500 + last_sync_length;
+		int is_new_pulse = delta > so->pulselength_min_sync /*1500*/ + last_sync_length;
 
 		so->did_handle_ootx = 0;
 
 		if( is_new_pulse )
 		{
-			int is_master_sync_pulse = delta > 40000;
+			int is_master_sync_pulse = delta > so->pulse_in_clear_time /*40000*/; 
 
 			if( is_master_sync_pulse )
 			{
@@ -110,7 +108,7 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
 
 
 	//See if this is a valid actual pulse.
-	else if( le->length < 1800 && le->length > 40 && delta > 30000 && ssn >= 0 )
+	else if( le->length < so->pulse_max_for_sweep && delta > so->pulse_in_clear_time && ssn >= 0 )
 	{
 		int32_t dl = so->last_time[0];
 		int32_t tpco = so->last_length[0];
@@ -123,10 +121,13 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
 		//Adding length 
 		//Long pulse-code from IR flood.
 		//Make sure it fits nicely into a divisible-by-500 time.
+
+		int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz.
+
 		int32_t acode_array[2] =
 			{
-				(so->last_length[0]+125+50)/250,
-				(so->last_length[1]+125+50)/250,
+				(so->last_length[0]+main_divisor+50)/(main_divisor*2),  //+50 adds a small offset and seems to help always get it right. 
+				(so->last_length[1]+main_divisor+50)/(main_divisor*2),	//Check the +50 in the future to see how well this works on a variety of hardware.
 			};
 
 		//XXX: TODO: Capture error count here.
@@ -150,14 +151,19 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
 			so->recent_sync_time = so->last_time[1];
 
 			//Throw out everything if our sync pulses look like they're bad.
-			if( delta1 < 375000 || delta1 > 385000 )
+
+			int32_t center_1 = so->timecenter_ticks*2 - so->pulse_synctime_offset;
+			int32_t center_2 = so->pulse_synctime_offset;
+			int32_t slack = so->pulse_synctime_slack;
+
+			if( delta1 < center_1 - slack || delta1 > center_1 + slack )
 			{
 				//XXX: TODO: Count faults.
 				so->sync_set_number = -1;
 				return;
 			}
 
-			if( delta2 < 15000 || delta2 > 25000 )
+			if( delta2 < center_2 - slack || delta2 > center_2 + slack )
 			{
 				//XXX: TODO: Count faults.
 				so->sync_set_number = -1;
diff --git a/src/survive_internal.h b/src/survive_internal.h
index 11c9d89..0d0b8a4 100644
--- a/src/survive_internal.h
+++ b/src/survive_internal.h
@@ -86,10 +86,12 @@ struct SurviveContext
 	struct libusb_device_handle * udev[MAX_USB_DEVS];
 	struct SurviveUSBInterface uiface[MAX_INTERFACES];
 
-	text_feedback_fnptr faultfunction;
-	text_feedback_fnptr notefunction;
+	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];
@@ -112,9 +114,6 @@ int survive_get_config( char ** config, struct SurviveContext * ctx, int devno,
 //Accept Data from backend.
 void survive_data_cb( struct SurviveUSBInterface * si );
 
-//Accept higher-level data.
-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 cb53588..75453da 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -21,18 +21,36 @@ void survive_default_light_process( struct SurviveObject * so, int sensor_id, in
 
 	//No loner need sync information past this point.
 	if( sensor_id < 0 ) return;
-
-	float angle = (timeinsweep - 200000) * (1./200000. * 3.14159265359/2.0);
+	FLT angle = (timeinsweep - so->timecenter_ticks) * (1./so->timecenter_ticks * 3.14159265359/2.0);
 
 	//Need to now do angle correction.
 #if 1
 	struct 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];
+//	angle += bsd->fcaltilt[axis] * predicted_angle(axis1);
+	
 	//TODO!!!
 #endif
-	
+
+	FLT length_sec = length / (FLT)so->timebase_hz;
+	ctx->angleproc( so, sensor_id, acode, timecode, length_sec, angle );
 }
 
+
+void survive_default_angle_process( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle )
+{
+	struct SurviveContext * ctx = so->ctx;
+	if( ctx->calptr )
+	{
+		survive_cal_angle( so, sensor_id, acode, timecode, length, angle );
+	}
+
+	//TODO: Writeme!
+}	
+
+
 void survive_default_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id )
 {
 	//TODO: Writeme!