diff options
| -rw-r--r-- | calibrate.c | 2 | ||||
| -rw-r--r-- | src/survive_cal.c | 47 | ||||
| -rw-r--r-- | 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; }; |