From 93873b616394b24fefb0ce17ae0e302ff2697d14 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Tue, 21 Feb 2017 02:54:42 -0500 Subject: making progress --- src/survive_cal.c | 88 +++++++++++++++++++++++++++++++++++++++--------- src/survive_cal.h | 17 ++++++++-- src/survive_cal_lhfind.c | 22 ++++++++++++ 3 files changed, 110 insertions(+), 17 deletions(-) create mode 100644 src/survive_cal_lhfind.c (limited to 'src') diff --git a/src/survive_cal.c b/src/survive_cal.c index d4c3447..62cf698 100644 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -29,7 +29,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); @@ -71,6 +71,11 @@ int survive_cal_get_status( struct SurviveContext * ctx, char * description, int { 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, "%d Unkown calibration state", cd->stage ); } @@ -183,23 +188,26 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin if( cd->peak_counts >= PTS_BEFORE_COMMON ) { - SV_INFO( "Stage 2 cal: %d %d %d\n", cd->peak_counts, cd->found_common, cd->times_found_common ); + int tfc = cd->times_found_common; if( cd->found_common ) { - if( cd->times_found_common >= NEEDED_TIMES_OF_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 { - cd->times_found_common++; + 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; } @@ -236,10 +244,20 @@ static void handle_calibration( struct SurviveCalData *cd ) { 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( ctsweeps, 0, sizeof( 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", 0666 ); + mkdir( "calinfo", 0755 ); FILE * hists = fopen( "calinfo/histograms.csv", "w" ); FILE * ptinfo = fopen( "calinfo/ptinfo.csv", "w" ); int sen, axis, lh; @@ -263,9 +281,9 @@ static void handle_calibration( struct SurviveCalData *cd ) 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 + #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; @@ -322,11 +340,10 @@ static void handle_calibration( struct SurviveCalData *cd ) FLT stddevlen = 0; #define HISTOGRAMSIZE 31 - #define HISTOGRAMBINANG 0.001 //TODO: Tune + #define HISTOGRAMBINANG 0.00001 //TODO: Tune int histo[HISTOGRAMSIZE]; memset( histo, 0, sizeof( histo ) ); - count = 0; for( i = 0; i < dpmax; i++ ) { @@ -354,11 +371,11 @@ static void handle_calibration( struct SurviveCalData *cd ) if( stddevang > ANGLE_STDEV_TOO_HIGH ) { - SV_INFO( "DROPPED: %02d dropped because stddev (%f) was too high.\n", sen, stddevang ); + SV_INFO( "DROPPED: %02d:%d:%d dropped because stddev (%f) was too high.", sen, lh, axis, stddevang ); continue; } - fprintf( hists, "%02d, ", sen ); + fprintf( hists, "%02d_%d_%d, ", sen, lh, axis ); for( i = 0; i < HISTOGRAMSIZE; i++ ) { @@ -366,13 +383,54 @@ static void handle_calibration( struct SurviveCalData *cd ) } 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 ); + 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*4+lh*2+axis; + avgsweeps[dataindex] = avgsweep; + avglens[dataindex] = avglen; + stdsweeps[dataindex] = stddevang; + stdlens[dataindex] = stddevlen; + ctsweeps[dataindex] = count; } fclose( hists ); fclose( ptinfo ); - //XXX TODO More - reset_calibration( cd ); + //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 = ctsweeps[sen*4+0]; + int ct1 = ctsweeps[sen*4+0]; + int ct2 = ctsweeps[sen*4+0]; + int ct3 = 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 ); + } } diff --git a/src/survive_cal.h b/src/survive_cal.h index dd69b57..bf6161d 100644 --- a/src/survive_cal.h +++ b/src/survive_cal.h @@ -32,14 +32,14 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin #define MAX_SENSORS_TO_CAL 96 #define DRPTS 512 - +#define MAX_CAL_PT_DAT (MAX_SENSORS_TO_CAL*NUM_LIGHTHOUSES*2) 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. + //For statistics-gathering phase. (Stage 2/3) 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]; @@ -47,6 +47,15 @@ struct SurviveCalData int8_t found_common; int8_t times_found_common; + //For camfind (4+) + 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]; + + int senid_of_checkpt; //This is a point on a watchman that can be used to check the lh solution. + //Stage: // 0: Idle // 1: Collecting OOTX data. @@ -54,6 +63,10 @@ struct SurviveCalData }; +//The following function is not included in the core survive_cal and must be compiled from a camfind file. +//It should use data for stage 4 and report if it found the +int survive_cal_lhfind( struct SurviveCalData * cd ); + #endif diff --git a/src/survive_cal_lhfind.c b/src/survive_cal_lhfind.c new file mode 100644 index 0000000..d879f30 --- /dev/null +++ b/src/survive_cal_lhfind.c @@ -0,0 +1,22 @@ +#include "survive_cal.h" + +//Stub file for doing lhfind (this doesn't work) + +//Return +int survive_cal_lhfind( struct SurviveCalData * cd ) +{ + struct SurviveContext * ctx = cd->ctx; + + //Use the following: + // 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]; + // + // Check your solution against point: senid_of_checkpt's data. + + return -1; //Return 0 if success. +} + + -- cgit v1.2.3