From e1818d84e1ab0ae217d8035f8a12398f06508059 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 17 Mar 2017 16:32:18 -0700 Subject: Alternate disambiguator and calibration updates --- src/survive_cal.c | 50 +++++++++++++++++++++++++++++++++++++++----------- 1 file changed, 39 insertions(+), 11 deletions(-) (limited to 'src/survive_cal.c') diff --git a/src/survive_cal.c b/src/survive_cal.c index 0eb9446..c36a48a 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -30,6 +30,11 @@ int mkdir(const char *); #define DRPTS_NEEDED_FOR_AVG ((int)(DRPTS*3/4)) + //at stage 1, is when it branches to stage two or stage 7 + //stage 2 checks for the presence of two watchmen and an HMD visible to both lighthouses. + //Stage 3 collects a bunch of data for statistical averageing + //stage 4 does the calculation for the poses (NOT DONE!) + //Stage 5 = System Calibrate.d static void handle_calibration( struct SurviveCalData *cd ); @@ -117,17 +122,36 @@ void survive_cal_install( struct SurviveContext * ctx ) cd->stage = 1; cd->ctx = ctx; - cd->poseobjects[0] = survive_get_so_by_name( ctx, "HMD" ); - cd->poseobjects[1] = survive_get_so_by_name( ctx, "WM0" ); - cd->poseobjects[2] = survive_get_so_by_name( ctx, "WM1" ); + cd->numPoseObjects = 0; - if( cd->poseobjects[0] == 0 || cd->poseobjects[1] == 0 || cd->poseobjects[2] == 0 ) + for (int i=0; i < ctx->objs_ct; i++) { - SV_ERROR( "Error: cannot find all devices needed for calibration." ); - free( cd ); - return; + // This would be a place where we could conditionally decide if we + // want to include a certain device in the calibration routine. + if (1) + { + cd->poseobjects[i] = ctx->objs[i]; + cd->numPoseObjects++; + + SV_INFO("Calibration is using %s", cd->poseobjects[i]->codename); + } } + // If we want to mandate that certain devices have been found + + + + //cd->poseobjects[0] = survive_get_so_by_name( ctx, "HMD" ); + //cd->poseobjects[1] = survive_get_so_by_name( ctx, "WM0" ); + //cd->poseobjects[2] = survive_get_so_by_name( ctx, "WM1" ); + + //if( cd->poseobjects[0] == 0 || cd->poseobjects[1] == 0 || cd->poseobjects[2] == 0 ) + //{ + // SV_ERROR( "Error: cannot find all devices needed for calibration." ); + // free( cd ); + // return; + //} + //XXX TODO MWTourney, work on your code here. /* if( !cd->hmd ) @@ -185,7 +209,9 @@ 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 && so->codename[0] == 'H' ) + // Take the OOTX data from the first device. + // TODO: Improve this so we'll watch all devices looking for OOTX data. Why limit ourselves to just one? + if( lhid < NUM_LIGHTHOUSES && so == cd->poseobjects[0] ) { uint8_t dbit = (acode & 2)>>1; ootx_pump_bit( &cd->ootx_decoders[lhid], dbit ); @@ -193,7 +219,9 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int 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. + //if( i == NUM_LIGHTHOUSES ) cd->stage = 2; //If all lighthouses have their OOTX set, move on. <------- Revert This!!!!! + if( i == 1 ) + cd->stage = 2; //If all lighthouses have their OOTX set, move on. } break; } @@ -498,11 +526,11 @@ static void handle_calibration( struct SurviveCalData *cd ) int obj; //Poses of lighthouses relative to objects. - SurvivePose objphl[POSE_OBJECTS][NUM_LIGHTHOUSES]; + SurvivePose objphl[MAX_POSE_OBJECTS][NUM_LIGHTHOUSES]; FILE * fobjp = fopen( "calinfo/objposes.csv", "w" ); - for( obj = 0; obj < POSE_OBJECTS; obj++ ) + for( obj = 0; obj < cd->numPoseObjects; obj++ ) { int i, j; PoserDataFullScene fsd; -- cgit v1.2.3 From 472d05c2a356ec6d71669d67fb599e401a6f4a76 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Thu, 23 Mar 2017 00:25:25 -0700 Subject: Updated disambiguator. Solid OOTX --- src/survive_cal.c | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) (limited to 'src/survive_cal.c') diff --git a/src/survive_cal.c b/src/survive_cal.c index c36a48a..51d12dd 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -190,7 +190,7 @@ void survive_cal_install( struct SurviveContext * ctx ) } -void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ) +void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, int lh) { struct SurviveContext * ctx = so->ctx; struct SurviveCalData * cd = ctx->calptr; @@ -210,7 +210,6 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int { int lhid = -sensor_id-1; // Take the OOTX data from the first device. - // TODO: Improve this so we'll watch all devices looking for OOTX data. Why limit ourselves to just one? if( lhid < NUM_LIGHTHOUSES && so == cd->poseobjects[0] ) { uint8_t dbit = (acode & 2)>>1; @@ -219,9 +218,9 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int 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. <------- Revert This!!!!! - if( i == 1 ) - cd->stage = 2; //If all lighthouses have their OOTX set, move on. + if( i == NUM_LIGHTHOUSES ) cd->stage = 2; //If all lighthouses have their OOTX set, move on. <------- Revert This!!!!! + //if( i == 1 ) + //cd->stage = 2; //If all lighthouses have their OOTX set, move on. } break; } -- cgit v1.2.3 From 280a6599fea76a7d2c16cfe0fcc5c8f37fde66de Mon Sep 17 00:00:00 2001 From: mwturvey Date: Thu, 23 Mar 2017 09:47:38 -0700 Subject: More cleanup --- src/survive_cal.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/survive_cal.c') diff --git a/src/survive_cal.c b/src/survive_cal.c index 51d12dd..dfa2e85 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -190,7 +190,7 @@ void survive_cal_install( struct SurviveContext * ctx ) } -void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, int lh) +void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh) { struct SurviveContext * ctx = so->ctx; struct SurviveCalData * cd = ctx->calptr; -- cgit v1.2.3 From f33018188bf5bdf6f6b8af0d646e3f8c519d9d71 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Thu, 23 Mar 2017 12:13:44 -0700 Subject: Added support for empty string in config.json & other cleanup. --- src/survive_cal.c | 50 +++++++++++++++++++++----------------------------- 1 file changed, 21 insertions(+), 29 deletions(-) (limited to 'src/survive_cal.c') diff --git a/src/survive_cal.c b/src/survive_cal.c index 19eb3ca..f9ec595 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -124,50 +124,42 @@ void survive_cal_install( struct SurviveContext * ctx ) cd->numPoseObjects = 0; - for (int i=0; i < ctx->objs_ct; i++) + const char * RequiredTrackersForCal = config_read_str( ctx->global_config_values, "RequiredTrackersForCal", "HMD,WM0,WM1" ); + const uint32_t AllowAllTrackersForCal = config_read_uint32( ctx->global_config_values, "AllowAllTrackersForCal", 0 ); + size_t requiredTrackersFound = 0; + + for (int j=0; j < ctx->objs_ct; j++) { - // This would be a place where we could conditionally decide if we - // want to include a certain device in the calibration routine. - if (1) + // Add the tracker if we allow all trackers for calibration, or if it's in the list + // of required trackers. + int isRequiredTracker = strstr(RequiredTrackersForCal, ctx->objs[j]->codename) != NULL; + + if (isRequiredTracker) { - cd->poseobjects[i] = ctx->objs[i]; + requiredTrackersFound++; + } + + if (AllowAllTrackersForCal || isRequiredTracker) + { + cd->poseobjects[j] = ctx->objs[j]; cd->numPoseObjects++; - SV_INFO("Calibration is using %s", cd->poseobjects[i]->codename); + SV_INFO("Calibration is using %s", cd->poseobjects[j]->codename); } + } // If we want to mandate that certain devices have been found - - - //cd->poseobjects[0] = survive_get_so_by_name( ctx, "HMD" ); - //cd->poseobjects[1] = survive_get_so_by_name( ctx, "WM0" ); - //cd->poseobjects[2] = survive_get_so_by_name( ctx, "WM1" ); - - //if( cd->poseobjects[0] == 0 || cd->poseobjects[1] == 0 || cd->poseobjects[2] == 0 ) - //{ - // SV_ERROR( "Error: cannot find all devices needed for calibration." ); - // free( cd ); - // return; - //} - -//XXX TODO MWTourney, work on your code here. -/* - if( !cd->hmd ) + if (strlen(RequiredTrackersForCal) > 0) { - cd->hmd = survive_get_so_by_name( ctx, "TR0" ); - - if( !cd->hmd ) + if (requiredTrackersFound != ((strlen(RequiredTrackersForCal) + 1) / 4)) { - SV_ERROR( "Error: cannot find any devices labeled HMD. Required for calibration" ); + SV_ERROR( "Error: cannot find all devices needed for calibration." ); free( cd ); return; } - SV_INFO( "HMD not found, calibrating using Tracker" ); } -*/ - const char * DriverName; const char * PreferredPoser = config_read_str( ctx->global_config_values, "ConfigPoser", "PoserCharlesSlow" ); -- cgit v1.2.3 From 3d86dda66d50d1e1955d4e0cd5f374e0aed1789f Mon Sep 17 00:00:00 2001 From: mwturvey Date: Thu, 23 Mar 2017 16:26:27 -0700 Subject: Fixed Windows USB Interface Enumeration --- src/survive_cal.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/survive_cal.c') diff --git a/src/survive_cal.c b/src/survive_cal.c index f9ec595..81889ca 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -155,7 +155,7 @@ void survive_cal_install( struct SurviveContext * ctx ) { if (requiredTrackersFound != ((strlen(RequiredTrackersForCal) + 1) / 4)) { - SV_ERROR( "Error: cannot find all devices needed for calibration." ); + SV_ERROR( "Error: Did not find all devices required for calibration." ); free( cd ); return; } @@ -201,7 +201,7 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int if( sensor_id < 0 ) { int lhid = -sensor_id-1; - // Take the OOTX data from the first device. + // Take the OOTX data from the first device. (if using HMD, WM0, WM1 only, this will be HMD) if( lhid < NUM_LIGHTHOUSES && so == cd->poseobjects[0] ) { uint8_t dbit = (acode & 2)>>1; -- cgit v1.2.3 From a576703242adea11c012af5afdff38af84d22e2e Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 24 Mar 2017 13:44:38 -0700 Subject: Adaptive Acode Offset Compensation --- src/survive_cal.c | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) (limited to 'src/survive_cal.c') diff --git a/src/survive_cal.c b/src/survive_cal.c index 81889ca..22a8eff 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -200,6 +200,7 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int //Collecting OOTX data. if( sensor_id < 0 ) { + //fprintf(stderr, "%s\n", so->codename); int lhid = -sensor_id-1; // Take the OOTX data from the first device. (if using HMD, WM0, WM1 only, this will be HMD) if( lhid < NUM_LIGHTHOUSES && so == cd->poseobjects[0] ) @@ -210,9 +211,7 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int 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. <------- Revert This!!!!! - //if( i == 1 ) - //cd->stage = 2; //If all lighthouses have their OOTX set, move on. + if( i == NUM_LIGHTHOUSES ) cd->stage = 2; //TODO: Make this configuratble to allow single lighthouse. } break; case 3: //Look for light sync lengths. -- cgit v1.2.3 From 4dc1d72785c660c206f8def9d8c8aa32289c2709 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 24 Mar 2017 15:19:59 -0700 Subject: More cleanup & finishing genericization of calibrator --- src/survive_cal.c | 30 +++++++++++++++++++----------- 1 file changed, 19 insertions(+), 11 deletions(-) (limited to 'src/survive_cal.c') diff --git a/src/survive_cal.c b/src/survive_cal.c index 22a8eff..6c153b4 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -220,10 +220,13 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int else if( acode < -4 ) break; int lh = (-acode) - 3; - if( strcmp( so->codename, "WM0" ) == 0 ) - sensor_id += 32; - if( strcmp( so->codename, "WM1" ) == 0 ) - sensor_id += 64; + for (int i=0; i < min(MAX_DEVICES_TO_CAL, cd->numPoseObjects); i++) + { + if( strcmp( so->codename, cd->poseobjects[i]->codename ) == 0 ) + { + sensor_id += i*32; + } + } cd->all_sync_times[sensor_id][lh][cd->all_sync_counts[sensor_id][lh]++] = length; break; @@ -233,7 +236,7 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int } -void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ) +void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh ) { struct SurviveContext * ctx = so->ctx; struct SurviveCalData * cd = ctx->calptr; @@ -241,14 +244,18 @@ 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" ) == 0 ) - sensid += 64; + + for (int i=0; i < min(MAX_DEVICES_TO_CAL, cd->numPoseObjects); i++) + { + if( strcmp( so->codename, cd->poseobjects[i]->codename ) == 0 ) + { + sensid += i*32; + } + } if( sensid >= MAX_SENSORS_TO_CAL || sensid < 0 ) return; - int lighthouse = acode>>2; + int lighthouse = lh; int axis = acode & 1; switch( cd->stage ) @@ -292,7 +299,8 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin int min_peaks = PTS_BEFORE_COMMON; int i, j, k; cd->found_common = 1; - for( i = 0; i < MAX_SENSORS_TO_CAL/SENSORS_PER_OBJECT; i++ ) + for( i = 0; i < cd->numPoseObjects; i++ ) + //for( i = 0; i < MAX_SENSORS_TO_CAL/SENSORS_PER_OBJECT; i++ ) for( j = 0; j < NUM_LIGHTHOUSES; j++ ) { int sensors_visible = 0; -- cgit v1.2.3 From a279a202768e1cae10f4fb651aad2b3b21db43ab Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 24 Mar 2017 15:36:43 -0700 Subject: remove min() --- src/survive_cal.c | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) (limited to 'src/survive_cal.c') diff --git a/src/survive_cal.c b/src/survive_cal.c index 6c153b4..5414048 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -141,10 +141,17 @@ void survive_cal_install( struct SurviveContext * ctx ) if (AllowAllTrackersForCal || isRequiredTracker) { - cd->poseobjects[j] = ctx->objs[j]; - cd->numPoseObjects++; + if (MAX_DEVICES_TO_CAL < cd->numPoseObjects) + { + cd->poseobjects[j] = ctx->objs[j]; + cd->numPoseObjects++; - SV_INFO("Calibration is using %s", cd->poseobjects[j]->codename); + SV_INFO("Calibration is using %s", cd->poseobjects[j]->codename); + } + else + { + SV_ERROR("Calibration is NOT using %s; device count exceeds MAX_DEVICES_TO_CAL", cd->poseobjects[j]->codename); + } } } @@ -220,7 +227,7 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int else if( acode < -4 ) break; int lh = (-acode) - 3; - for (int i=0; i < min(MAX_DEVICES_TO_CAL, cd->numPoseObjects); i++) + for (int i=0; i < cd->numPoseObjects; i++) { if( strcmp( so->codename, cd->poseobjects[i]->codename ) == 0 ) { @@ -234,6 +241,7 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int } + } void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh ) @@ -245,7 +253,7 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin int sensid = sensor_id; - for (int i=0; i < min(MAX_DEVICES_TO_CAL, cd->numPoseObjects); i++) + for (int i=0; i < cd->numPoseObjects; i++) { if( strcmp( so->codename, cd->poseobjects[i]->codename ) == 0 ) { -- cgit v1.2.3 From efe1b3e98866d45dd3bb5f8060712db152762edd Mon Sep 17 00:00:00 2001 From: Michael Turvey Date: Fri, 24 Mar 2017 16:00:20 -0700 Subject: Fixing linux --- src/survive_cal.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/survive_cal.c') diff --git a/src/survive_cal.c b/src/survive_cal.c index 5414048..ae92bad 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -141,7 +141,7 @@ void survive_cal_install( struct SurviveContext * ctx ) if (AllowAllTrackersForCal || isRequiredTracker) { - if (MAX_DEVICES_TO_CAL < cd->numPoseObjects) + if (MAX_DEVICES_TO_CAL > cd->numPoseObjects) { cd->poseobjects[j] = ctx->objs[j]; cd->numPoseObjects++; @@ -150,7 +150,7 @@ void survive_cal_install( struct SurviveContext * ctx ) } else { - SV_ERROR("Calibration is NOT using %s; device count exceeds MAX_DEVICES_TO_CAL", cd->poseobjects[j]->codename); + SV_INFO("Calibration is NOT using %s; device count exceeds MAX_DEVICES_TO_CAL", ctx->objs[j]->codename); } } -- cgit v1.2.3