From 5a11716edeb85bc01c38bfc4ce174d101cac0a06 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Tue, 21 Feb 2017 01:59:08 -0500 Subject: Working on it, not quite tested. --- src/survive_cal.c | 231 +++++++++++++++++++++++++++++++++++++++++++++++------ src/survive_cal.h | 10 ++- src/survive_data.c | 3 +- 3 files changed, 216 insertions(+), 28 deletions(-) (limited to 'src') diff --git a/src/survive_cal.c b/src/survive_cal.c index 7b0a824..d4c3447 100644 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -2,14 +2,23 @@ // (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 #include +#include +#include #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 ); @@ -49,20 +58,21 @@ 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 ); } default: - return snprintf( description, description_length, "Unkown calibration state" ); + return snprintf( description, description_length, "%d Unkown calibration state", cd->stage ); } } @@ -79,6 +89,7 @@ void survive_cal_install( struct SurviveContext * ctx ) } cd->stage = 1; + cd->ctx = ctx; ootx_packet_clbk = ootx_packet_clbk_d; @@ -127,6 +138,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" ) == 1 ) + 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 +157,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 +181,45 @@ 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 ); - } + SV_INFO( "Stage 2 cal: %d %d %d\n", cd->peak_counts, cd->found_common, cd->times_found_common ); + if( cd->found_common ) + { + if( cd->times_found_common >= NEEDED_TIMES_OF_COMMON ) + { + reset_calibration( cd ); + cd->stage = 3; + cd->found_common = 1; + } + else + { + cd->times_found_common++; + reset_calibration( cd ); + } + } + else + { + 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 +228,153 @@ 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; + + //Either advance to stage 4 or go resetting will go back to stage 2. + //What is stage 4? Are we done then? + + mkdir( "calinfo", 0666 ); + 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.01 //TODO: Tune + #define OUTLIER_LENGTH 0.01 //TODO: Tune + #define ANGLE_STDEV_TOO_HIGH 0.01 //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 ) + { + //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.001 //TODO: Tune + + int histo[HISTOGRAMSIZE]; + memset( histo, 0, sizeof( histo ) ); + count = 0; + + 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 dropped because stddev (%f) was too high.\n", sen, stddevang ); + continue; + } + + fprintf( hists, "%02d, ", sen ); + + for( i = 0; i < HISTOGRAMSIZE; i++ ) + { + fprintf( hists, "%d ", histo[i] ); + } + fprintf( hists, "\n" ); + + fprintf( ptinfo, "%d %d %f %f %f %f %f %f\n", sen, count, avgsweep, avglen, stddevang, stddevang, max_outlier_length, max_outlier_angle ); + } + fclose( hists ); + fclose( ptinfo ); + //XXX TODO More reset_calibration( cd ); } + + + + diff --git a/src/survive_cal.h b/src/survive_cal.h index 42ff1ee..dd69b57 100644 --- a/src/survive_cal.h +++ b/src/survive_cal.h @@ -30,20 +30,22 @@ 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 MAX_SENSORS_TO_CAL 96 #define DRPTS 512 struct SurviveCalData { + struct SurviveContext * ctx; //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]; + FLT all_lengths[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES][2][DRPTS]; + FLT all_angles[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES][2][DRPTS]; + int16_t all_counts[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES][2]; int16_t peak_counts; int8_t found_common; + int8_t times_found_common; //Stage: // 0: Idle diff --git a/src/survive_data.c b/src/survive_data.c index ad834cf..402282d 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -177,7 +177,8 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement * if (acode > 3) { if( ssn == 0 ) { - SV_INFO( "Warning: got a slave marker but only got a master sync." ); + //SV_INFO( "Warning: got a slave marker but only got a master sync." ); + //This happens too frequently. Consider further examination. } dl = so->last_time[1]; tpco = so->last_length[1]; -- cgit v1.2.3