From bd89d46cb01f7069166e85f017f169e07acc7094 Mon Sep 17 00:00:00 2001
From: cnlohr <lohr85@gmail.com>
Date: Thu, 16 Feb 2017 18:28:54 -0500
Subject: closing in on finding common cal points.

---
 calibrate.c       |  2 +-
 src/survive_cal.c | 47 ++++++++++++++++++++++++++++++++++++++++++-----
 src/survive_cal.h | 13 +++++++------
 3 files changed, 50 insertions(+), 12 deletions(-)

diff --git a/calibrate.c b/calibrate.c
index 2db544a..60f4316 100644
--- a/calibrate.c
+++ b/calibrate.c
@@ -126,7 +126,7 @@ void * GuiThread( void * v )
 		survive_cal_get_status( ctx, caldesc, sizeof( caldesc ) );
 		CNFGPenX = 3;
 		CNFGPenY = 3;
-		CNFGDrawText( caldesc, 2 );
+		CNFGDrawText( caldesc, 4 );
 
 
 		CNFGSwapBuffers();
diff --git a/src/survive_cal.c b/src/survive_cal.c
index 8069154..7b0a824 100644
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -8,7 +8,11 @@
 #include <math.h>
 #include <string.h>
 
+#define PTS_BEFORE_COMMON 32
+#define NEEDED_COMMON_POINTS 20
+
 static void handle_calibration( struct SurviveCalData *cd );
+static void reset_calibration( struct SurviveCalData * cd );
 
 void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet)
 {
@@ -49,7 +53,14 @@ int survive_cal_get_status( struct SurviveContext * ctx, char * description, int
 	case 1:
 		return snprintf( description, description_length, "Collecting OOTX Data (%d:%d)", cd->ootx_decoders[0].buf_offset, cd->ootx_decoders[1].buf_offset );
 	case 2:
-		return snprintf( description, description_length, "Collecting Sweep Data %d/%d", cd->peak_counts, DRPTS );
+		if( cd->found_common )
+		{
+			return snprintf( description, description_length, "Collecting Sweep Data %d/%d", cd->peak_counts, DRPTS );
+		}
+		else
+		{
+			return snprintf( description, description_length, "Searching for common watchman cal %d/%d", cd->peak_counts, PTS_BEFORE_COMMON );
+		}
 	default:
 		return snprintf( description, description_length, "Unkown calibration state" );
 	}
@@ -141,18 +152,44 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin
 			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 )
+		{
+			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;
+					break;
+				}
+			}
+			if( ok ) cd->found_common = 1;
+		}
+
+		if( cd->peak_counts > PTS_BEFORE_COMMON && !cd->found_common )
+		{
+			reset_calibration( cd );
+		}
+
 		break;
 	}
 	}
 }
 
+static void reset_calibration( struct SurviveCalData * cd )
+{
+	memset( cd->all_counts, 0, sizeof( cd->all_counts ) );
+	cd->peak_counts = 0;
+	cd->found_common = 0;
+}
 
 static void handle_calibration( struct SurviveCalData *cd )
 {
 	//Do stuff.
 
-	memset( cd->all_lengths, 0, sizeof( cd->all_lengths ) );
-	memset( cd->all_angles, 0, sizeof( cd->all_angles ) );
-	memset( cd->all_counts, 0, sizeof( cd->all_counts ) );
-	cd->peak_counts = 0;
+	reset_calibration( cd );
 }
diff --git a/src/survive_cal.h b/src/survive_cal.h
index b9d4b11..42ff1ee 100644
--- a/src/survive_cal.h
+++ b/src/survive_cal.h
@@ -35,11 +35,6 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin
 
 struct SurviveCalData
 {
-	//Stage:
-	// 0: Idle
-	// 1: Collecting OOTX data.
-	int stage;
-
 	//OOTX Data is sync'd off of the sync pulses coming from the lighthouses.
 	ootx_decoder_context ootx_decoders[NUM_LIGHTHOUSES];
 
@@ -47,7 +42,13 @@ struct SurviveCalData
 	FLT all_lengths[MAX_TO_CAL][NUM_LIGHTHOUSES][2][DRPTS];
 	FLT all_angles[MAX_TO_CAL][NUM_LIGHTHOUSES][2][DRPTS];
 	int16_t all_counts[MAX_TO_CAL][NUM_LIGHTHOUSES][2];
-	int peak_counts;
+	int16_t peak_counts;
+	int8_t found_common;
+
+	//Stage:
+	// 0: Idle
+	// 1: Collecting OOTX data.
+	int8_t stage;
 };
 
 
-- 
cgit v1.2.3