From 35e08aaa24bd01e6ace453f89ddb73a6bb0508b0 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Tue, 14 Feb 2017 01:00:33 -0500 Subject: Still not working, but getting closer to dynamic OOTX decoding. --- src/survive_cal.c | 68 +++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 68 insertions(+) create mode 100644 src/survive_cal.c (limited to 'src/survive_cal.c') diff --git a/src/survive_cal.c b/src/survive_cal.c new file mode 100644 index 0000000..4223e70 --- /dev/null +++ b/src/survive_cal.c @@ -0,0 +1,68 @@ +// (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. + +#include "survive_cal.h" +#include "survive_internal.h" + +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; + + printf( "Got OOTX packet %d %p\n", id, cd ); +} + + +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; + + 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-1 && so->codename[0] == 'H' ) + { + uint8_t dbit = (acode & 2)?0xff:0x00; + printf( "%s %d %d %d\n", so->codename, lhid, acode, dbit ); + ootx_process_bit( &cd->ootx_decoders[lhid], dbit ); + } + } + + break; + } +} + -- cgit v1.2.3 From 119a205619632076c7b258eaa6c28dd7bcd2c294 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Tue, 14 Feb 2017 10:03:18 -0500 Subject: integrate josh's new work. It works! #19 is just about solved! --- src/survive_cal.c | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) (limited to 'src/survive_cal.c') diff --git a/src/survive_cal.c b/src/survive_cal.c index 4223e70..adcb7bc 100644 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -13,6 +13,10 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet) int id = ct->user1; printf( "Got OOTX packet %d %p\n", id, cd ); + + lighthouse_info_v6 v6; + init_lighthouse_info_v6(&v6, packet->data); + print_lighthouse_info_v6(&v6); } @@ -54,11 +58,11 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int if( sensor_id < 0 ) { int lhid = -sensor_id-1; - if( lhid >= NUM_LIGHTHOUSES-1 && so->codename[0] == 'H' ) + if( lhid < NUM_LIGHTHOUSES && so->codename[0] == 'H' ) { - uint8_t dbit = (acode & 2)?0xff:0x00; - printf( "%s %d %d %d\n", so->codename, lhid, acode, dbit ); - ootx_process_bit( &cd->ootx_decoders[lhid], dbit ); + uint8_t dbit = (acode & 2)>>1; + //printf( "%s %d %d %d\n", so->codename, lhid, acode, dbit ); + ootx_pump_bit( &cd->ootx_decoders[lhid], dbit ); } } -- cgit v1.2.3 From 2e5d0355da2376f27dcbe0cc6d04b737145ac853 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Thu, 16 Feb 2017 15:47:46 -0500 Subject: update with more integration stuff. --- src/survive_cal.c | 27 ++++++++++++++++++++++++--- 1 file changed, 24 insertions(+), 3 deletions(-) (limited to 'src/survive_cal.c') diff --git a/src/survive_cal.c b/src/survive_cal.c index adcb7bc..cdc7783 100644 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -12,11 +12,26 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet) struct SurviveCalData * cd = ctx->calptr; int id = ct->user1; - printf( "Got OOTX packet %d %p\n", id, cd ); + SV_INFO( "Got OOTX packet %d %p\n", id, cd ); lighthouse_info_v6 v6; init_lighthouse_info_v6(&v6, packet->data); - print_lighthouse_info_v6(&v6); + + 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] = v6.fcal_0_tilt; + b->fcaltilt[1] = v6.fcal_1_tilt; + 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; } @@ -64,8 +79,14 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int //printf( "%s %d %d %d\n", so->codename, lhid, acode, dbit ); 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: break; } } -- cgit v1.2.3 From 32fbccbd7d90f1e456d1e477eab2128aaf88df93 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Thu, 16 Feb 2017 17:16:31 -0500 Subject: Move to having an angle callback. --- src/survive_cal.c | 26 +++++++++++++++++++++++--- 1 file changed, 23 insertions(+), 3 deletions(-) (limited to 'src/survive_cal.c') diff --git a/src/survive_cal.c b/src/survive_cal.c index cdc7783..6bb8173 100644 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -5,6 +5,7 @@ #include "survive_cal.h" #include "survive_internal.h" +#include void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet) { @@ -23,8 +24,8 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet) b->BaseStationID = v6.id; b->fcalphase[0] = v6.fcal_0_phase; b->fcalphase[1] = v6.fcal_1_phase; - b->fcaltilt[0] = v6.fcal_0_tilt; - b->fcaltilt[1] = v6.fcal_1_tilt; + 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; @@ -76,7 +77,6 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int if( lhid < NUM_LIGHTHOUSES && so->codename[0] == 'H' ) { uint8_t dbit = (acode & 2)>>1; - //printf( "%s %d %d %d\n", so->codename, lhid, acode, dbit ); ootx_pump_bit( &cd->ootx_decoders[lhid], dbit ); } int i; @@ -85,9 +85,29 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int 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; + switch( cd->stage ) + { + default: + case 1: //Collecting OOTX data. (Don't do anything here, yet.) + case 0: //Default, inactive. + break; case 2: + //if( sensor_id == 0 && so->codename[0] == 'H' ) printf( "%d %f %f\n", acode, length, angle ); break; } } + + -- cgit v1.2.3 From 9bba1f9e7888f512a587f76179b9dd8f389c7ae8 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Thu, 16 Feb 2017 18:09:19 -0500 Subject: Start collecting data. Getting closer to having a full cal stack. --- src/survive_cal.c | 47 ++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 46 insertions(+), 1 deletion(-) (limited to 'src/survive_cal.c') 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 +#include + +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; +} -- cgit v1.2.3 From bd89d46cb01f7069166e85f017f169e07acc7094 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Thu, 16 Feb 2017 18:28:54 -0500 Subject: closing in on finding common cal points. --- src/survive_cal.c | 47 ++++++++++++++++++++++++++++++++++++++++++----- 1 file changed, 42 insertions(+), 5 deletions(-) (limited to 'src/survive_cal.c') 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 #include +#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 ); } -- cgit v1.2.3