diff options
| -rw-r--r-- | calibrate.c | 4 | ||||
| -rw-r--r-- | data_recorder.c | 4 | ||||
| -rw-r--r-- | include/libsurvive/survive.h | 3 | ||||
| -rw-r--r-- | include/libsurvive/survive_types.h | 2 | ||||
| -rwxr-xr-x | src/survive_cal.c | 51 | ||||
| -rw-r--r-- | src/survive_cal.h | 8 | ||||
| -rw-r--r-- | src/survive_data.c | 262 | ||||
| -rw-r--r-- | src/survive_process.c | 6 |
8 files changed, 311 insertions, 29 deletions
diff --git a/calibrate.c b/calibrate.c index f27131a..62acbf6 100644 --- a/calibrate.c +++ b/calibrate.c @@ -51,10 +51,10 @@ int bufferpts[32*2*3][2]; char buffermts[32*128*3]; int buffertimeto[32*3][2]; -void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ) +void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh) { // if( timeinsweep < 0 ) return; - survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length ); + survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length, lh); if( sensor_id < 0 ) return; if( acode < 0 ) return; //return; diff --git a/data_recorder.c b/data_recorder.c index 46f3427..04a219a 100644 --- a/data_recorder.c +++ b/data_recorder.c @@ -43,9 +43,9 @@ int bufferpts[32*2*3]; char buffermts[32*128*3]; int buffertimeto[32*3]; -void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ) +void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh) { - survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length ); + survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length, lh); if( acode == -1 ) return; //return; diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index e04586c..1165e9d 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -47,6 +47,7 @@ struct SurviveObject int32_t pulse_synctime_slack; //5,000 for normal vive hardware. (guessed) //Flood info, for calculating which laser is currently sweeping. + void * disambiguator_data; int8_t oldcode; int8_t sync_set_number; //0 = master, 1 = slave, -1 = fault. int8_t did_handle_ootx; //If unset, will send lightcap data for sync pulses next time a sensor is hit. @@ -129,7 +130,7 @@ void survive_cal_install( SurviveContext * ctx ); //XXX This will be removed if //Call these from your callback if overridden. //Accept higher-level data. -void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); +void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length , uint32_t lh); void survive_default_imu_process( SurviveObject * so, int mode, FLT * accelgyro, uint32_t timecode, int id ); void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h index 1600e11..def30b8 100644 --- a/include/libsurvive/survive_types.h +++ b/include/libsurvive/survive_types.h @@ -28,7 +28,7 @@ typedef struct BaseStationData BaseStationData; typedef struct SurviveCalData SurviveCalData; //XXX Warning: This may be removed. Check at a later time for its defunctness. typedef void (*text_feedback_func)( SurviveContext * ctx, const char * fault ); -typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); +typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lighthouse); typedef void (*imu_process_func)( SurviveObject * so, int mask, FLT * accelgyro, uint32_t timecode, int id ); typedef void (*angle_process_func)( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); diff --git a/src/survive_cal.c b/src/survive_cal.c index 04931cc..19eb3ca 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 ) @@ -166,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, uint32_t lh) { struct SurviveContext * ctx = so->ctx; struct SurviveCalData * cd = ctx->calptr; @@ -185,7 +209,8 @@ 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. + if( lhid < NUM_LIGHTHOUSES && so == cd->poseobjects[0] ) { uint8_t dbit = (acode & 2)>>1; ootx_pump_bit( &cd->ootx_decoders[lhid], dbit ); @@ -193,7 +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. + 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; case 3: //Look for light sync lengths. @@ -552,11 +579,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; diff --git a/src/survive_cal.h b/src/survive_cal.h index 45b77f6..8f4e4de 100644 --- a/src/survive_cal.h +++ b/src/survive_cal.h @@ -29,7 +29,7 @@ int survive_cal_get_status( SurviveContext * ctx, char * description, int descri //void survive_cal_teardown( struct SurviveContext * ctx ); //Called from survive_default_light_process -void survive_cal_light( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); +void survive_cal_light( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lighthouse); void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); #define MAX_SENSORS_TO_CAL 96 @@ -39,7 +39,7 @@ void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t t #define DRPTS 32 //Number of samples required in collection phase. -#define POSE_OBJECTS 3 +#define MAX_POSE_OBJECTS 10 #define MAX_CAL_PT_DAT (MAX_SENSORS_TO_CAL*NUM_LIGHTHOUSES*2) struct SurviveCalData @@ -69,7 +69,9 @@ struct SurviveCalData int senid_of_checkpt; //This is a point on a watchman that can be used to check the lh solution. - SurviveObject * poseobjects[POSE_OBJECTS]; + SurviveObject * poseobjects[MAX_POSE_OBJECTS]; + + size_t numPoseObjects; PoserCB ConfigPoserFn; diff --git a/src/survive_data.c b/src/survive_data.c index 0873f7f..ee180b1 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -5,6 +5,255 @@ #include <stdint.h> #include <string.h> +typedef struct +{ + unsigned int sweep_time[SENSORS_PER_OBJECT]; + unsigned int sweep_len[SENSORS_PER_OBJECT]; +} lightcaps_sweep_data; +typedef struct +{ + int recent_sync_time; + int activeLighthouse; + int activeSweepStartTime; + int activeAcode; + + int lh_pulse_len[NUM_LIGHTHOUSES]; + int lh_start_time[NUM_LIGHTHOUSES]; + int current_lh; // used knowing which sync pulse we're looking at. + +} lightcap2_global_data; + +typedef struct +{ + lightcaps_sweep_data sweep; + lightcap2_global_data global; +} lightcap2_data; + + +//static lightcap2_global_data lcgd = { 0 }; + +int handle_lightcap2_getAcodeFromSyncPulse(int pulseLen) +{ + if (pulseLen < 3125) return 0; + if (pulseLen < 3625) return 1; + if (pulseLen < 4125) return 2; + if (pulseLen < 4625) return 3; + if (pulseLen < 5125) return 4; + if (pulseLen < 5625) return 5; + if (pulseLen < 6125) return 6; + return 7; +} +void handle_lightcap2_process_sweep_data(SurviveObject *so) +{ + lightcap2_data *lcd = so->disambiguator_data; + + // look at all of the sensors we found, and process the ones that were hit. + // TODO: find the sensor(s) with the longest pulse length, and assume + // those are the "highest quality". Then, reject any pulses that are sufficiently + // different from those values, assuming that they are reflections. + { + unsigned int longest_pulse = 0; + unsigned int timestamp_of_longest_pulse = 0; + for (int i = 0; i < SENSORS_PER_OBJECT; i++) + { + if (lcd->sweep.sweep_len[i] > longest_pulse) + { + longest_pulse = lcd->sweep.sweep_len[i]; + timestamp_of_longest_pulse = lcd->sweep.sweep_time[i]; + } + } + + for (int i = 0; i < SENSORS_PER_OBJECT; i++) + { + if (lcd->sweep.sweep_len[i] != 0) // if the sensor was hit, process it + { + int offset_from = lcd->sweep.sweep_time[i] - lcd->global.activeSweepStartTime + lcd->sweep.sweep_len[i] / 2; + + if (offset_from < 380000 && offset_from > 70000) + { + if (longest_pulse *10 / 8 < lcd->sweep.sweep_len[i]) + { + so->ctx->lightproc(so, i, lcd->global.activeAcode, offset_from, lcd->sweep.sweep_time[i], lcd->sweep.sweep_len[i], lcd->global.activeLighthouse); + } + } + } + } + } + // clear out sweep data (could probably limit this to only after a "first" sync. + // this is slightly more robust, so doing it here for now. + memset(&(((lightcap2_data*)so->disambiguator_data)->sweep), 0, sizeof(lightcaps_sweep_data)); +} +void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) +{ + //fprintf(stderr, "%6.6d %4.4d \n", le->timestamp - so->recent_sync_time, le->length); + lightcap2_data *lcd = so->disambiguator_data; + + //static unsigned int recent_sync_time = 0; + //static unsigned int recent_sync_count = -1; + //static unsigned int activeSweepStartTime; + + + // Process any sweep data we have + handle_lightcap2_process_sweep_data(so); + + int time_since_last_sync = (le->timestamp - lcd->global.recent_sync_time); + + //fprintf(stderr, " %2d %8d %d\n", le->sensor_id, time_since_last_sync, le->length); + // need to store up sync pulses, so we can take the earliest starting time for all sensors. + if (time_since_last_sync < 2400) + { + lcd->global.recent_sync_time = le->timestamp; + // it's the same sync pulse; + so->sync_set_number = 1; + so->recent_sync_time = le->timestamp; + + lcd->global.lh_pulse_len[lcd->global.current_lh] = le->length; + lcd->global.lh_start_time[lcd->global.current_lh] = le->timestamp; + + int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); + if (!(acode >> 2 & 1)) // if the skip bit is not set + { + lcd->global.activeLighthouse = lcd->global.current_lh; + lcd->global.activeSweepStartTime = le->timestamp; + lcd->global.activeAcode = acode; + } + else + { + lcd->global.activeLighthouse = -1; + lcd->global.activeSweepStartTime = 0; + lcd->global.activeAcode = 0; + } + } + else if (time_since_last_sync < 24000) + { + lcd->global.recent_sync_time = le->timestamp; + // I do believe we are lighthouse B + lcd->global.current_lh = 1; + lcd->global.lh_pulse_len[lcd->global.current_lh] = le->length; + lcd->global.lh_start_time[lcd->global.current_lh] = le->timestamp; + + int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); + + if (!(acode >> 2 & 1)) // if the skip bit is not set + { + if (lcd->global.activeLighthouse != -1) + { + // hmm, it appears we got two non-skip pulses at the same time. That should never happen + fprintf(stderr, "WARNING: Two non-skip pulses received on the same cycle!\n"); + } + lcd->global.activeLighthouse = 1; + lcd->global.activeSweepStartTime = le->timestamp; + lcd->global.activeAcode = acode; + } + + } + else if (time_since_last_sync > 370000) + { + // looks like this is the first sync pulse. Cool! + + // first, send out the sync pulse data for the last round (for OOTX decoding + { + if (lcd->global.lh_pulse_len[0] != 0) + { + so->ctx->lightproc( + so, + -1, + handle_lightcap2_getAcodeFromSyncPulse(lcd->global.lh_pulse_len[0]), + lcd->global.lh_pulse_len[0], + lcd->global.lh_start_time[0], + 0, + 0); + } + if (lcd->global.lh_pulse_len[1] != 0) + { + so->ctx->lightproc( + so, + -2, + handle_lightcap2_getAcodeFromSyncPulse(lcd->global.lh_pulse_len[1]), + lcd->global.lh_pulse_len[1], + lcd->global.lh_start_time[1], + 0, + 1); + } + } + + // initialize here. + memset(&lcd->global, 0, sizeof(lcd->global)); + lcd->global.activeLighthouse = -1; + + + + lcd->global.recent_sync_time = le->timestamp; + // I do believe we are lighthouse A + lcd->global.current_lh = 0; + lcd->global.lh_pulse_len[lcd->global.current_lh] = le->length; + lcd->global.lh_start_time[lcd->global.current_lh] = le->timestamp; + + int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); + + if (!(acode >> 2 & 1)) // if the skip bit is not set + { + lcd->global.activeLighthouse = 0; + lcd->global.activeSweepStartTime = le->timestamp; + lcd->global.activeAcode = acode; + } + } +} + +void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le ) +{ + lightcap2_data *lcd = so->disambiguator_data; + + // If we see multiple "hits" on the sweep for a given sensor, + // assume that the longest (i.e. strongest signal) is most likely + // the non-reflected signal. + + if (le->length < 80) + { + // this is a low-quality read. Better to throw it out than to use it. + //fprintf(stderr, "%2d %d\n", le->sensor_id, le->length); + return; + } + fprintf(stderr, "%2d %d\n", le->sensor_id, le->length); + + if (lcd->sweep.sweep_len[le->sensor_id] < le->length) + { + lcd->sweep.sweep_len[le->sensor_id] = le->length; + lcd->sweep.sweep_time[le->sensor_id] = le->timestamp; + } +} + +void handle_lightcap2( SurviveObject * so, LightcapElement * le ) +{ + SurviveContext * ctx = so->ctx; + + if (so->disambiguator_data == NULL) + { + so->disambiguator_data = malloc(sizeof(lightcap2_data)); + memset(so->disambiguator_data, 0, sizeof(lightcap2_data)); + } + + if( le->sensor_id > SENSORS_PER_OBJECT ) + { + return; + } + + if (le->length > 6750) + { + // Should never get a reading so high. Odd. + return; + } + if (le->length >= 2750) + { + // Looks like a sync pulse, process it! + handle_lightcap2_sync(so, le); + return; + } + + // must be a sweep pulse, process it! + handle_lightcap2_sweep(so, le); + +} int32_t decode_acode(uint32_t length, int32_t main_divisor) { //+50 adds a small offset and seems to help always get it right. @@ -19,7 +268,10 @@ int32_t decode_acode(uint32_t length, int32_t main_divisor) { //This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode. void handle_lightcap( SurviveObject * so, LightcapElement * le ) { - SurviveContext * ctx = so->ctx; + SurviveContext * ctx = so->ctx; +// handle_lightcap2(so,le); +// return; + //int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time; if( le->sensor_id > SENSORS_PER_OBJECT ) @@ -121,7 +373,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) int base_station = is_new_pulse; //printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length ); ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length ); - } + } } @@ -160,8 +412,8 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) int32_t delta1 = so->last_sync_time[0] - so->recent_sync_time; int32_t delta2 = so->last_sync_time[1] - so->last_sync_time[0]; - ctx->lightproc( so, -1, acode_array[0], delta1, so->last_sync_time[0], so->last_sync_length[0] ); - ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1] ); + ctx->lightproc( so, -1, acode_array[0], delta1, so->last_sync_time[0], so->last_sync_length[0], 0 ); + ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1], 1 ); so->recent_sync_time = so->last_sync_time[1]; @@ -204,7 +456,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) //Make sure pulse is in valid window if( offset_from < 380000 && offset_from > 70000 ) { - ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length ); + ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length, so->sync_set_number ); } } else diff --git a/src/survive_process.c b/src/survive_process.c index 9295638..d4604d8 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -6,14 +6,14 @@ //XXX TODO: Once data is avialble in the context, use the stuff here to handle converting from time codes to //proper angles, then from there perform the rest of the solution. -void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ) +void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh) { SurviveContext * ctx = so->ctx; - int base_station = acode >> 2; + int base_station = lh; int axis = acode & 1; if( ctx->calptr ) { - survive_cal_light( so, sensor_id, acode, timeinsweep, timecode, length ); + survive_cal_light( so, sensor_id, acode, timeinsweep, timecode, length, lh); } //We don't use sync times, yet. |