aboutsummaryrefslogtreecommitdiff
path: root/src/survive_cal.c
diff options
context:
space:
mode:
Diffstat (limited to 'src/survive_cal.c')
-rw-r--r--src/survive_cal.c438
1 files changed, 438 insertions, 0 deletions
diff --git a/src/survive_cal.c b/src/survive_cal.c
new file mode 100644
index 0000000..62cf698
--- /dev/null
+++ b/src/survive_cal.c
@@ -0,0 +1,438 @@
+// (C) 2016, 2017 Joshua Allen, MIT/x11 License.
+// (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 <math.h>
+#include <string.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+
+#define PTS_BEFORE_COMMON 32
+#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 );
+
+void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet)
+{
+ struct SurviveContext * ctx = (struct SurviveContext*)(ct->user);
+ struct SurviveCalData * cd = ctx->calptr;
+ int id = ct->user1;
+
+ SV_INFO( "Got OOTX packet %d %p", id, cd );
+
+ lighthouse_info_v6 v6;
+ init_lighthouse_info_v6(&v6, packet->data);
+
+ struct BaseStationData * b = &ctx->bsd[id];
+ //print_lighthouse_info_v6(&v6);
+
+ b->BaseStationID = v6.id;
+ b->fcalphase[0] = v6.fcal_0_phase;
+ b->fcalphase[1] = v6.fcal_1_phase;
+ b->fcaltilt[0] = tan(v6.fcal_0_tilt);
+ b->fcaltilt[1] = tan(v6.fcal_1_tilt); //XXX??? Is this right? See https://github.com/cnlohr/libsurvive/issues/18
+ b->fcalcurve[0] = v6.fcal_0_curve;
+ b->fcalcurve[1] = v6.fcal_1_curve;
+ b->fcalgibpha[0] = v6.fcal_0_gibphase;
+ b->fcalgibpha[1] = v6.fcal_1_gibphase;
+ b->fcalgibmag[0] = v6.fcal_0_gibmag;
+ b->fcalgibmag[1] = v6.fcal_1_gibmag;
+ 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, "0 Not calibrating" );
+ case 1:
+ 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, "%d Collecting Sweep Data %d/%d", cd->stage, cd->peak_counts, DRPTS );
+ }
+ else
+ {
+ 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 );
+ }
+}
+
+void survive_cal_install( struct SurviveContext * ctx )
+{
+ int i;
+ struct SurviveCalData * cd = ctx->calptr = calloc( 1, sizeof( struct SurviveCalData ) );
+
+ for( i = 0; i < NUM_LIGHTHOUSES; i++ )
+ {
+ ootx_init_decoder_context(&cd->ootx_decoders[i]);
+ cd->ootx_decoders[i].user = ctx;
+ cd->ootx_decoders[i].user1 = i;
+ }
+
+ cd->stage = 1;
+ cd->ctx = ctx;
+
+ ootx_packet_clbk = ootx_packet_clbk_d;
+
+ ctx->calptr = cd;
+}
+
+
+void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length )
+{
+ struct SurviveContext * ctx = so->ctx;
+ struct SurviveCalData * cd = ctx->calptr;
+
+ if( !cd ) return;
+
+ switch( cd->stage )
+ {
+ default:
+ case 0: //Default, inactive.
+ break;
+
+ case 1:
+ //Collecting OOTX data.
+ if( sensor_id < 0 )
+ {
+ int lhid = -sensor_id-1;
+ if( lhid < NUM_LIGHTHOUSES && so->codename[0] == 'H' )
+ {
+ uint8_t dbit = (acode & 2)>>1;
+ ootx_pump_bit( &cd->ootx_decoders[lhid], dbit );
+ }
+ int i;
+ for( i = 0; i < NUM_LIGHTHOUSES; i++ )
+ if( ctx->bsd[i].OOTXSet == 0 ) break;
+ if( i == NUM_LIGHTHOUSES ) cd->stage = 2; //If all lighthouses have their OOTX set, move on.
+ }
+ break;
+ case 2: //Taking in angle data.
+ break;
+ }
+}
+
+void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle )
+{
+ struct SurviveContext * ctx = so->ctx;
+ struct SurviveCalData * cd = ctx->calptr;
+
+ 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:
+ case 1: //Collecting OOTX data. (Don't do anything here, yet.)
+ case 0: //Default, inactive.
+ break;
+ case 2:
+ {
+ 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;
+ }
+
+ //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;
+ break;
+ }
+ }
+ if( ok ) cd->found_common = 1;
+ }
+
+ if( cd->peak_counts >= PTS_BEFORE_COMMON )
+ {
+ int tfc = cd->times_found_common;
+ if( cd->found_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
+ {
+ 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;
+ }
+ }
+
+ 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;
+ }
+ }
+}
+
+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 )
+{
+ 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", 0755 );
+ 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.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;
+ 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.00001 //TODO: Tune
+
+ int histo[HISTOGRAMSIZE];
+ memset( histo, 0, sizeof( histo ) );
+
+ 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:%d:%d dropped because stddev (%f) was too high.", sen, lh, axis, stddevang );
+ continue;
+ }
+
+ fprintf( hists, "%02d_%d_%d, ", sen, lh, axis );
+
+ for( i = 0; i < HISTOGRAMSIZE; i++ )
+ {
+ fprintf( hists, "%d ", histo[i] );
+ }
+ fprintf( hists, "\n" );
+
+ 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 );
+
+ //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 );
+ }
+}
+
+
+
+