Start collecting data. Getting closer to having a full cal stack.

2 files changed, 59 insertions, 3 deletions

diff --git a/src/survive_cal.c b/src/survive_cal.c
index 6bb8173..8069154 100644
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -6,6 +6,9 @@
 #include "survive_cal.h"
 #include "survive_internal.h"
 #include <math.h>
+#include <string.h>
+
+static void handle_calibration( struct SurviveCalData *cd );
 
 void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet)
 {
@@ -35,6 +38,22 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet)
 	b->OOTXSet = 1;
 }
 
+int survive_cal_get_status( struct SurviveContext * ctx, char * description, int description_length )
+{
+	struct SurviveCalData * cd = ctx->calptr;
+
+	switch( cd->stage )
+	{
+	case 0:
+		return snprintf( description, description_length, "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 );
+	case 2:
+		return snprintf( description, description_length, "Collecting Sweep Data %d/%d", cd->peak_counts, DRPTS );
+	default:
+		return snprintf( description, description_length, "Unkown calibration state" );
+	}
+}
 
 void survive_cal_install( struct SurviveContext * ctx )
 {
@@ -104,10 +123,36 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin
 	case 0: //Default, inactive.
 		break;
 	case 2:
-		//if( sensor_id == 0 && so->codename[0] == 'H' ) printf( "%d %f %f\n", acode, length, angle );
+	{
+		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.
+		}
 		break;
 	}
+	}
 }
 
 
+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;
+}
diff --git a/src/survive_cal.h b/src/survive_cal.h
index 6653a84..b9d4b11 100644
--- a/src/survive_cal.h
+++ b/src/survive_cal.h
@@ -22,7 +22,7 @@
 #include "survive_internal.h"
 
 void survive_cal_install( struct SurviveContext * ctx );
-int survive_cal_get_status( struct SurviveContext * ctx, char * description, int max_data );
+int survive_cal_get_status( struct SurviveContext * ctx, char * description, int description_length );
 
 //void survive_cal_teardown( struct SurviveContext * ctx );
 
@@ -30,6 +30,9 @@ int survive_cal_get_status( struct SurviveContext * ctx, char * description, int
 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 );
 
+#define MAX_TO_CAL 96
+#define DRPTS 512
+
 struct SurviveCalData
 {
 	//Stage:
@@ -37,9 +40,17 @@ struct SurviveCalData
 	// 1: Collecting OOTX data.
 	int stage;
 
-	//OOTX Data is sync'd off of 
+	//OOTX Data is sync'd off of the sync pulses coming from the lighthouses.
 	ootx_decoder_context ootx_decoders[NUM_LIGHTHOUSES];
+
+	//For statistics-gathering phase.
+	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;
 };
 
+
+
 #endif