1 files changed, 284 insertions, 25 deletions

diff --git a/src/survive_cal.c b/src/survive_cal.c
index 7b0a824..f372309 100644
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -2,14 +2,25 @@
 // (C) 2016, 2017 <>< C. N. Lohr, Under MIT/x11 License.
 
 // All OOTX code was written by J. Allen. Rest of the code is probably mostly CNLohr.
+//
+// This file is primarily geared to the calibration phase, to produce the world cal information.
+// Once world cal is produced, it's unlikely you will need this file at all.  The plan is
+// to not include it at all on any stripped-down versions of libsurvive.
+//
 
 #include "survive_cal.h"
 #include "survive_internal.h"
 #include <math.h>
 #include <string.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+
+#include "survive_config.h"
 
 #define PTS_BEFORE_COMMON 32
-#define NEEDED_COMMON_POINTS 20
+#define NEEDED_COMMON_POINTS 10
+#define NEEDED_TIMES_OF_COMMON 5
+#define DRPTS_NEEDED_FOR_AVG ((int)(DRPTS*3/4))
 
 static void handle_calibration( struct SurviveCalData *cd );
 static void reset_calibration( struct SurviveCalData * cd );
@@ -20,7 +31,7 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet)
 	struct SurviveCalData * cd = ctx->calptr;
 	int id = ct->user1;
 
-	SV_INFO( "Got OOTX packet %d %p\n", id, cd );
+	SV_INFO( "Got OOTX packet %d %p", id, cd );
 
 	lighthouse_info_v6 v6;
 	init_lighthouse_info_v6(&v6, packet->data);
@@ -40,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 )
@@ -49,20 +63,26 @@ int survive_cal_get_status( struct SurviveContext * ctx, char * description, int
 	switch( cd->stage )
 	{
 	case 0:
-		return snprintf( description, description_length, "Not calibrating" );
+		return snprintf( description, description_length, "0 Not calibrating" );
 	case 1:
-		return snprintf( description, description_length, "Collecting OOTX Data (%d:%d)", cd->ootx_decoders[0].buf_offset, cd->ootx_decoders[1].buf_offset );
+		return snprintf( description, description_length, "1 Collecting OOTX Data (%d:%d)", cd->ootx_decoders[0].buf_offset, cd->ootx_decoders[1].buf_offset );
 	case 2:
+	case 3:
 		if( cd->found_common )
 		{
-			return snprintf( description, description_length, "Collecting Sweep Data %d/%d", cd->peak_counts, DRPTS );
+			return snprintf( description, description_length, "%d Collecting Sweep Data %d/%d", cd->stage, cd->peak_counts, DRPTS );
 		}
 		else
 		{
-			return snprintf( description, description_length, "Searching for common watchman cal %d/%d", cd->peak_counts, PTS_BEFORE_COMMON );
+			return snprintf( description, description_length, "%d Searching for common watchman cal %d/%d (%d/%d)", cd->stage, cd->peak_counts, PTS_BEFORE_COMMON, cd->times_found_common, NEEDED_TIMES_OF_COMMON );
 		}
+
+	case 5:
+		return snprintf( description, description_length, "%d LH Find complete.", cd->stage );
+
+	case 4:
 	default:
-		return snprintf( description, description_length, "Unkown calibration state" );
+		return snprintf( description, description_length, "%d Unkown calibration state", cd->stage );
 	}
 }
 
@@ -79,6 +99,15 @@ void survive_cal_install( struct SurviveContext * ctx )
 	}
 
 	cd->stage = 1;
+	cd->ctx = ctx;
+
+	cd->hmd = survive_get_so_by_name( ctx, "HMD" );
+	if( !cd->hmd )
+	{
+		SV_ERROR( "Error: cannot find any devices labeled HMD. Required for calibration" );
+		free( cd );
+		return;
+	}
 
 	ootx_packet_clbk = ootx_packet_clbk_d;
 
@@ -127,6 +156,17 @@ 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;
+
+	if( sensid >= MAX_SENSORS_TO_CAL || sensid < 0 ) return;
+
+	int lighthouse = acode>>2;
+	int axis = acode & 1;
+
 	switch( cd->stage )
 	{
 	default:
@@ -135,32 +175,21 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin
 		break;
 	case 2:
 	{
-		int sensid = sensor_id;
-		if( strcmp( so->codename, "WM0" ) == 0 )
-			sensid += 32;
-		if( strcmp( so->codename, "WM1" ) == 1 )
-			sensid += 64;
-
-		int lighthouse = acode>>2;
-		int axis = acode & 1;
 		int ct = cd->all_counts[sensid][lighthouse][axis]++;
 		cd->all_lengths[sensid][lighthouse][axis][ct] = length;
 		cd->all_angles[sensid][lighthouse][axis][ct] = angle;
 		if( ct > cd->peak_counts )
 		{
 			cd->peak_counts = ct;
-			if( ct >= DRPTS )
-				handle_calibration( cd ); //This will also reset all cals.
 		}
 
-		//TODO: Determine if there is a sensor on a watchman visible from both lighthouses.
-		if( sensid >= 32 && !cd->found_common )
+		//Determine if there is a sensor on a watchman visible from both lighthouses.
+		if( sensid >= 32 )
 		{
 			int k;
 			int ok = 1;
 			for( k = 0; k < NUM_LIGHTHOUSES; k++ )
 			{
-
 				if( cd->all_counts[sensid][k][0] < NEEDED_COMMON_POINTS || cd->all_counts[sensid][k][1] < NEEDED_COMMON_POINTS )
 				{
 					ok = 0;
@@ -170,13 +199,48 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin
 			if( ok ) cd->found_common = 1;
 		}
 
-		if( cd->peak_counts > PTS_BEFORE_COMMON && !cd->found_common )
+		if( cd->peak_counts >= PTS_BEFORE_COMMON )
 		{
-			reset_calibration( cd );
-		}
+			int tfc = cd->times_found_common;
+			if( cd->found_common )
+			{
+				if( tfc >= NEEDED_TIMES_OF_COMMON )
+				{
+					SV_INFO( "Stage 2 moving to stage 3. %d %d %d", cd->peak_counts, cd->found_common, tfc );
+					reset_calibration( cd );
+					cd->stage = 3;
+					cd->found_common = 1;
+				}
+				else
+				{
+					SV_INFO( "Stage 2 good - continuing. %d %d %d", cd->peak_counts, cd->found_common, tfc );
+					reset_calibration( cd );
+					cd->times_found_common = tfc+1;
+				}
+			}
+			else
+			{
+				SV_INFO( "Stage 2 bad - redoing. %d %d %d", cd->peak_counts, cd->found_common, tfc );
+				reset_calibration( cd );
+				cd->times_found_common = 0;
+			}
+		}			
 
 		break;
 	}
+	case 3:
+	{
+		int ct = cd->all_counts[sensid][lighthouse][axis]++;
+		cd->all_lengths[sensid][lighthouse][axis][ct] = length;
+		cd->all_angles[sensid][lighthouse][axis][ct] = angle;
+		if( ct > cd->peak_counts )
+		{
+			cd->peak_counts = ct;
+			if( ct >= DRPTS )
+				handle_calibration( cd ); //This must also reset all cals.
+		}
+		break;
+	}
 	}
 }
 
@@ -185,11 +249,206 @@ static void reset_calibration( struct SurviveCalData * cd )
 	memset( cd->all_counts, 0, sizeof( cd->all_counts ) );
 	cd->peak_counts = 0;
 	cd->found_common = 0;
+	cd->times_found_common = 0;
+	cd->stage = 2;
 }
 
 static void handle_calibration( struct SurviveCalData *cd )
 {
-	//Do stuff.
+	struct SurviveContext * ctx = cd->ctx;
+
+	#define MAX_CAL_PT_DAT (MAX_SENSORS_TO_CAL*NUM_LIGHTHOUSES*2)
+
+/*
+	FLT avgsweeps[MAX_CAL_PT_DAT];
+	FLT avglens[MAX_CAL_PT_DAT];
+	FLT stdsweeps[MAX_CAL_PT_DAT];
+	FLT stdlens[MAX_CAL_PT_DAT];
+	int ctsweeps[MAX_CAL_PT_DAT];
+*/
+
+	memset( cd->ctsweeps, 0, sizeof( cd->ctsweeps ) );
+
+	//Either advance to stage 4 or go resetting will go back to stage 2.
+	//What is stage 4?  Are we done then?
+
+	mkdir( "calinfo", 0755 );
+	FILE * hists = fopen( "calinfo/histograms.csv", "w" );
+	FILE * ptinfo = fopen( "calinfo/ptinfo.csv", "w" );
+	int sen, axis, lh;
+	for( sen = 0; sen < MAX_SENSORS_TO_CAL; sen++ )
+	for( lh = 0; lh < NUM_LIGHTHOUSES; lh++ )
+	for( axis = 0; axis < 2; axis++ )
+	{
+		int dpmax = cd->all_counts[sen][lh][axis];
+		if( dpmax < 50 ) continue;
+		int i;
+
+		FLT sumsweepangle = 0;
+		FLT sumlentime = 0;
+
+		//Find initial guess at average
+		for( i = 0; i < dpmax; i++ )
+		{
+			FLT sweepangle = cd->all_angles[sen][lh][axis][i];
+			FLT datalen = cd->all_lengths[sen][lh][axis][i];
+			sumsweepangle += sweepangle;
+			sumlentime += datalen;
+		}
+
+		#define OUTLIER_ANGLE   0.001	//TODO: Tune
+		#define OUTLIER_LENGTH	0.001	//TODO: Tune
+		#define ANGLE_STDEV_TOO_HIGH 0.000001 //TODO: Tune
+
+		FLT avgsweep = sumsweepangle / dpmax;
+		FLT avglen = sumlentime / dpmax;
+		int count = 0;
+
+		FLT max_outlier_angle = 0;
+		FLT max_outlier_length = 0;
+
+		//Get rid of outliers
+		for( i = 0; i < dpmax; i++ )
+		{
+			FLT sweepangle = cd->all_angles[sen][lh][axis][i];
+			FLT datalen = cd->all_lengths[sen][lh][axis][i];
+			FLT Sdiff = sweepangle - avgsweep;
+			FLT Ldiff = datalen - avglen;
+			FLT Sdiff2 = Sdiff * Sdiff;
+			FLT Ldiff2 = Ldiff * Ldiff;
+
+			if( Sdiff2 > max_outlier_angle ) max_outlier_angle = Sdiff2;
+			if( Ldiff2 > max_outlier_length ) max_outlier_length = Ldiff2;
+
+			if( Sdiff2 > OUTLIER_ANGLE || Ldiff2 > OUTLIER_LENGTH )
+			{
+				cd->all_lengths[sen][lh][axis][i] = -1;
+			}
+			else
+			{
+				count++;
+			}
+		}
+
+		if( count < DRPTS_NEEDED_FOR_AVG )
+		{
+			printf( "DPAVG %d\n", count );
+			//Not enough for this point to be considered.
+			continue;
+		}
+
+		sumsweepangle = 0;
+		sumlentime = 0;
+		//Redo, finding new average:
+		for( i = 0; i < dpmax; i++ )
+		{
+			FLT sweepangle = cd->all_angles[sen][lh][axis][i];
+			FLT datalen = cd->all_lengths[sen][lh][axis][i];
+			if( datalen < 0 ) continue;
+			sumsweepangle += sweepangle;
+			sumlentime += datalen;
+		}
+
+		avgsweep = sumsweepangle / count;
+		avglen = sumlentime / count;
+
+		FLT stddevang = 0;
+		FLT stddevlen = 0;
+
+		#define HISTOGRAMSIZE   31
+		#define HISTOGRAMBINANG 0.00001  //TODO: Tune
+
+		int histo[HISTOGRAMSIZE];
+		memset( histo, 0, sizeof( histo ) );
+
+		for( i = 0; i < dpmax; i++ )
+		{
+			FLT sweepangle = cd->all_angles[sen][lh][axis][i];
+			FLT datalen = cd->all_lengths[sen][lh][axis][i];
+			if( datalen < 0 ) continue;
+
+			FLT Sdiff = sweepangle - avgsweep;
+			FLT Ldiff = datalen - avglen;
+			FLT Sdiff2 = Sdiff * Sdiff;
+			FLT Ldiff2 = Ldiff * Ldiff;
+
+			stddevang += Sdiff2;
+			stddevlen += Ldiff2;
+
+			int llm = Sdiff / HISTOGRAMBINANG + (HISTOGRAMSIZE/2.0);
+			if( llm < 0 ) llm = 0;
+			if( llm >= HISTOGRAMSIZE ) llm = HISTOGRAMSIZE-1;
+
+			histo[llm]++;
+		}
+
+		stddevang /= count;
+		stddevlen /= count;
+
+		if( stddevang > ANGLE_STDEV_TOO_HIGH )
+		{
+			SV_INFO( "DROPPED: %02d:%d:%d dropped because stddev (%f) was too high.", sen, lh, axis, stddevang );
+			continue;
+		}
+
+		fprintf( hists, "%02d_%d_%d, ", sen, lh, axis );
 
-	reset_calibration( cd );
+		for( i = 0; i < HISTOGRAMSIZE; i++ )
+		{
+			fprintf( hists, "%d ", histo[i] );
+		}
+		fprintf( hists, "\n" );
+
+		fprintf( ptinfo, "%d %d %d %d %f %f %f %f %f %f\n", sen, lh, axis, count, avgsweep, avglen*1000000, stddevang*1000000000, stddevlen*1000000000, max_outlier_length*1000000000, max_outlier_angle*1000000000 );
+
+		int dataindex = sen*(2*NUM_LIGHTHOUSES)+lh*2+axis;
+		cd->avgsweeps[dataindex] = avgsweep;
+		cd->avglens[dataindex] = avglen;
+		cd->stdsweeps[dataindex] = stddevang;
+		cd->stdlens[dataindex] = stddevlen;
+		cd->ctsweeps[dataindex] = count;
+	}
+	fclose( hists );
+	fclose( ptinfo );
+
+	//Comb through data and make sure we still have a sensor on a WM that 
+	int bcp_senid = 0;
+	int bcp_count = 0;
+	for( sen = 0; sen < MAX_SENSORS_TO_CAL; sen++ )
+	{
+		int ct0 = cd->ctsweeps[sen*4+0];
+		int ct1 = cd->ctsweeps[sen*4+0];
+		int ct2 = cd->ctsweeps[sen*4+0];
+		int ct3 = cd->ctsweeps[sen*4+0];
+
+		if( ct0 > ct1 ) ct0 = ct1;
+		if( ct0 > ct2 ) ct0 = ct2;
+		if( ct0 > ct3 ) ct0 = ct3;
+
+		if( ct0 > bcp_count ) { bcp_count = ct0; bcp_senid = sen; }
+	}
+
+	if( bcp_count < DRPTS_NEEDED_FOR_AVG )
+	{
+		SV_INFO( "Stage 3 could not find a suitable common point on a watchman" );
+		reset_calibration( cd );
+		return;
+	}
+
+	cd->senid_of_checkpt = bcp_senid;
+
+	if( survive_cal_lhfind( cd ) == 0 )
+	{
+		SV_INFO( "Stage 4 succeeded." );
+		cd->stage = 5;
+	}
+	else
+	{
+		SV_INFO( "Stage 4 failed." );
+		reset_calibration( cd );
+	}
 }
+
+
+
+