update with more integration stuff.

3 files changed, 72 insertions, 5 deletions

diff --git a/calibrate.c b/calibrate.c
index 4db2ed0..ce50b67 100644
--- a/calibrate.c
+++ b/calibrate.c
@@ -40,6 +40,36 @@ int buffertimeto[32*3];
 void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length  )
 {
 	survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length );
+
+	if( acode == -1 ) return;
+//return;
+	int jumpoffset = sensor_id;
+	if( strcmp( so->codename, "WM0" ) == 0 ) jumpoffset += 32;
+	else if( strcmp( so->codename, "WM1" ) == 0 ) jumpoffset += 64;
+
+
+	if( acode == 0 || acode == 2 ) //data = 0
+	{
+		bufferpts[jumpoffset*2+0] = (timeinsweep-100000)/500;
+		buffertimeto[jumpoffset] = 0;
+	}
+	if( acode == 1 || acode == 3 ) //data = 1
+	{
+		bufferpts[jumpoffset*2+1] = (timeinsweep-100000)/500;
+		buffertimeto[jumpoffset] = 0;
+	}
+
+
+	if( acode == 4 || acode == 6 ) //data = 0
+	{
+		bufferpts[jumpoffset*2+0] = (timeinsweep-100000)/500;
+		buffertimeto[jumpoffset] = 0;
+	}
+	if( acode == 5 || acode == 7 ) //data = 1
+	{
+		bufferpts[jumpoffset*2+1] = (timeinsweep-100000)/500;
+		buffertimeto[jumpoffset] = 0;
+	}
 }
 
 void my_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id )
diff --git a/src/survive_cal.c b/src/survive_cal.c
index adcb7bc..cdc7783 100644
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -12,11 +12,26 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet)
 	struct SurviveCalData * cd = ctx->calptr;
 	int id = ct->user1;
 
-	printf( "Got OOTX packet %d %p\n", id, cd );
+	SV_INFO( "Got OOTX packet %d %p\n", id, cd );
 
 	lighthouse_info_v6 v6;
 	init_lighthouse_info_v6(&v6, packet->data);
-	print_lighthouse_info_v6(&v6);
+
+	struct BaseStationData * b = &ctx->bsd[id];
+	//print_lighthouse_info_v6(&v6);
+
+	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->fcalcurve[0] = v6.fcal_0_curve;
+	b->fcalcurve[1] = v6.fcal_1_curve;
+	b->fcalgibpha[0] = v6.fcal_0_gibphase;
+	b->fcalgibpha[1] = v6.fcal_1_gibphase;
+	b->fcalgibmag[0] = v6.fcal_0_gibmag;
+	b->fcalgibmag[1] = v6.fcal_1_gibmag;
+	b->OOTXSet = 1;
 }
 
 
@@ -64,8 +79,14 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int
 				//printf( "%s %d %d %d\n", so->codename, lhid, acode, dbit );
 				ootx_pump_bit( &cd->ootx_decoders[lhid], dbit );
 			}
+			int i;
+			for( i = 0; i < NUM_LIGHTHOUSES; i++ )
+				if( ctx->bsd[i].OOTXSet == 0 ) break;
+			if( i == NUM_LIGHTHOUSES ) cd->stage = 2;  //If all lighthouses have their OOTX set, move on.
 		}
-			
+		break;
+
+	case 2:
 		break;
 	}
 }
diff --git a/src/survive_process.c b/src/survive_process.c
index 184532b..cb53588 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -8,13 +8,29 @@
 
 void survive_default_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length  )
 {
+	struct SurviveContext * ctx = so->ctx;
+	int base_station = acode >> 2;
+	int axis = acode & 1;
 
-	if( so->ctx->calptr )
+	if( ctx->calptr )
 	{
 		survive_cal_light( so, sensor_id, acode, timeinsweep, timecode, length );
 	}
 
-	//TODO: Writeme!
+	if( base_station > NUM_LIGHTHOUSES ) return;
+
+	//No loner need sync information past this point.
+	if( sensor_id < 0 ) return;
+
+	float angle = (timeinsweep - 200000) * (1./200000. * 3.14159265359/2.0);
+
+	//Need to now do angle correction.
+#if 1
+	struct BaseStationData * bsd = &ctx->bsd[base_station];
+
+	//TODO!!!
+#endif
+	
 }
 
 void survive_default_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id )