From 5a11716edeb85bc01c38bfc4ce174d101cac0a06 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Tue, 21 Feb 2017 01:59:08 -0500 Subject: Working on it, not quite tested. --- src/survive_cal.c | 231 +++++++++++++++++++++++++++++++++++++++++++++++------ src/survive_cal.h | 10 ++- src/survive_data.c | 3 +- 3 files changed, 216 insertions(+), 28 deletions(-) (limited to 'src') diff --git a/src/survive_cal.c b/src/survive_cal.c index 7b0a824..d4c3447 100644 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -2,14 +2,23 @@ // (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 #include +#include +#include #define PTS_BEFORE_COMMON 32 -#define NEEDED_COMMON_POINTS 20 +#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 ); @@ -49,20 +58,21 @@ int survive_cal_get_status( struct SurviveContext * ctx, char * description, int switch( cd->stage ) { case 0: - return snprintf( description, description_length, "Not calibrating" ); + return snprintf( description, description_length, "0 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 ); + 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, "Collecting Sweep Data %d/%d", cd->peak_counts, DRPTS ); + return snprintf( description, description_length, "%d Collecting Sweep Data %d/%d", cd->stage, cd->peak_counts, DRPTS ); } else { - return snprintf( description, description_length, "Searching for common watchman cal %d/%d", cd->peak_counts, PTS_BEFORE_COMMON ); + 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 ); } default: - return snprintf( description, description_length, "Unkown calibration state" ); + return snprintf( description, description_length, "%d Unkown calibration state", cd->stage ); } } @@ -79,6 +89,7 @@ void survive_cal_install( struct SurviveContext * ctx ) } cd->stage = 1; + cd->ctx = ctx; ootx_packet_clbk = ootx_packet_clbk_d; @@ -127,6 +138,17 @@ 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" ) == 1 ) + sensid += 64; + + if( sensid >= MAX_SENSORS_TO_CAL || sensid < 0 ) return; + + int lighthouse = acode>>2; + int axis = acode & 1; + switch( cd->stage ) { default: @@ -135,32 +157,21 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin break; case 2: { - 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. } - //TODO: Determine if there is a sensor on a watchman visible from both lighthouses. - if( sensid >= 32 && !cd->found_common ) + //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; @@ -170,13 +181,45 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin if( ok ) cd->found_common = 1; } - if( cd->peak_counts > PTS_BEFORE_COMMON && !cd->found_common ) + if( cd->peak_counts >= PTS_BEFORE_COMMON ) { - reset_calibration( cd ); - } + SV_INFO( "Stage 2 cal: %d %d %d\n", cd->peak_counts, cd->found_common, cd->times_found_common ); + if( cd->found_common ) + { + if( cd->times_found_common >= NEEDED_TIMES_OF_COMMON ) + { + reset_calibration( cd ); + cd->stage = 3; + cd->found_common = 1; + } + else + { + cd->times_found_common++; + reset_calibration( cd ); + } + } + else + { + 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; + } } } @@ -185,11 +228,153 @@ 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 ) { - //Do stuff. + struct SurviveContext * ctx = cd->ctx; + + //Either advance to stage 4 or go resetting will go back to stage 2. + //What is stage 4? Are we done then? + + mkdir( "calinfo", 0666 ); + 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.01 //TODO: Tune + #define OUTLIER_LENGTH 0.01 //TODO: Tune + #define ANGLE_STDEV_TOO_HIGH 0.01 //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.001 //TODO: Tune + + int histo[HISTOGRAMSIZE]; + memset( histo, 0, sizeof( histo ) ); + count = 0; + + 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 dropped because stddev (%f) was too high.\n", sen, stddevang ); + continue; + } + + fprintf( hists, "%02d, ", sen ); + + for( i = 0; i < HISTOGRAMSIZE; i++ ) + { + fprintf( hists, "%d ", histo[i] ); + } + fprintf( hists, "\n" ); + + fprintf( ptinfo, "%d %d %f %f %f %f %f %f\n", sen, count, avgsweep, avglen, stddevang, stddevang, max_outlier_length, max_outlier_angle ); + } + fclose( hists ); + fclose( ptinfo ); + //XXX TODO More reset_calibration( cd ); } + + + + diff --git a/src/survive_cal.h b/src/survive_cal.h index 42ff1ee..dd69b57 100644 --- a/src/survive_cal.h +++ b/src/survive_cal.h @@ -30,20 +30,22 @@ int survive_cal_get_status( struct SurviveContext * ctx, char * description, int void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); -#define MAX_TO_CAL 96 +#define MAX_SENSORS_TO_CAL 96 #define DRPTS 512 struct SurviveCalData { + struct SurviveContext * ctx; //OOTX Data is sync'd off of the sync pulses coming from the lighthouses. ootx_decoder_context ootx_decoders[NUM_LIGHTHOUSES]; //For statistics-gathering phase. - 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]; + FLT all_lengths[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES][2][DRPTS]; + FLT all_angles[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES][2][DRPTS]; + int16_t all_counts[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES][2]; int16_t peak_counts; int8_t found_common; + int8_t times_found_common; //Stage: // 0: Idle diff --git a/src/survive_data.c b/src/survive_data.c index ad834cf..402282d 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -177,7 +177,8 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement * if (acode > 3) { if( ssn == 0 ) { - SV_INFO( "Warning: got a slave marker but only got a master sync." ); + //SV_INFO( "Warning: got a slave marker but only got a master sync." ); + //This happens too frequently. Consider further examination. } dl = so->last_time[1]; tpco = so->last_length[1]; -- cgit v1.2.3 From 93873b616394b24fefb0ce17ae0e302ff2697d14 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Tue, 21 Feb 2017 02:54:42 -0500 Subject: making progress --- src/survive_cal.c | 88 +++++++++++++++++++++++++++++++++++++++--------- src/survive_cal.h | 17 ++++++++-- src/survive_cal_lhfind.c | 22 ++++++++++++ 3 files changed, 110 insertions(+), 17 deletions(-) create mode 100644 src/survive_cal_lhfind.c (limited to 'src') diff --git a/src/survive_cal.c b/src/survive_cal.c index d4c3447..62cf698 100644 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -29,7 +29,7 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet) struct SurviveCalData * cd = ctx->calptr; int id = ct->user1; - SV_INFO( "Got OOTX packet %d %p\n", id, cd ); + SV_INFO( "Got OOTX packet %d %p", id, cd ); lighthouse_info_v6 v6; init_lighthouse_info_v6(&v6, packet->data); @@ -71,6 +71,11 @@ int survive_cal_get_status( struct SurviveContext * ctx, char * description, int { 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 ); } @@ -183,23 +188,26 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin if( cd->peak_counts >= PTS_BEFORE_COMMON ) { - SV_INFO( "Stage 2 cal: %d %d %d\n", cd->peak_counts, cd->found_common, cd->times_found_common ); + int tfc = cd->times_found_common; if( cd->found_common ) { - if( cd->times_found_common >= NEEDED_TIMES_OF_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 { - cd->times_found_common++; + 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; } @@ -236,10 +244,20 @@ 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", 0666 ); + mkdir( "calinfo", 0755 ); FILE * hists = fopen( "calinfo/histograms.csv", "w" ); FILE * ptinfo = fopen( "calinfo/ptinfo.csv", "w" ); int sen, axis, lh; @@ -263,9 +281,9 @@ static void handle_calibration( struct SurviveCalData *cd ) sumlentime += datalen; } - #define OUTLIER_ANGLE 0.01 //TODO: Tune - #define OUTLIER_LENGTH 0.01 //TODO: Tune - #define ANGLE_STDEV_TOO_HIGH 0.01 //TODO: Tune + #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; @@ -322,11 +340,10 @@ static void handle_calibration( struct SurviveCalData *cd ) FLT stddevlen = 0; #define HISTOGRAMSIZE 31 - #define HISTOGRAMBINANG 0.001 //TODO: Tune + #define HISTOGRAMBINANG 0.00001 //TODO: Tune int histo[HISTOGRAMSIZE]; memset( histo, 0, sizeof( histo ) ); - count = 0; for( i = 0; i < dpmax; i++ ) { @@ -354,11 +371,11 @@ static void handle_calibration( struct SurviveCalData *cd ) if( stddevang > ANGLE_STDEV_TOO_HIGH ) { - SV_INFO( "DROPPED: %02d dropped because stddev (%f) was too high.\n", sen, stddevang ); + SV_INFO( "DROPPED: %02d:%d:%d dropped because stddev (%f) was too high.", sen, lh, axis, stddevang ); continue; } - fprintf( hists, "%02d, ", sen ); + fprintf( hists, "%02d_%d_%d, ", sen, lh, axis ); for( i = 0; i < HISTOGRAMSIZE; i++ ) { @@ -366,13 +383,54 @@ static void handle_calibration( struct SurviveCalData *cd ) } fprintf( hists, "\n" ); - fprintf( ptinfo, "%d %d %f %f %f %f %f %f\n", sen, count, avgsweep, avglen, stddevang, stddevang, max_outlier_length, max_outlier_angle ); + 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 ); - //XXX TODO More - reset_calibration( cd ); + //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 ); + } } diff --git a/src/survive_cal.h b/src/survive_cal.h index dd69b57..bf6161d 100644 --- a/src/survive_cal.h +++ b/src/survive_cal.h @@ -32,14 +32,14 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin #define MAX_SENSORS_TO_CAL 96 #define DRPTS 512 - +#define MAX_CAL_PT_DAT (MAX_SENSORS_TO_CAL*NUM_LIGHTHOUSES*2) struct SurviveCalData { struct SurviveContext * ctx; //OOTX Data is sync'd off of the sync pulses coming from the lighthouses. ootx_decoder_context ootx_decoders[NUM_LIGHTHOUSES]; - //For statistics-gathering phase. + //For statistics-gathering phase. (Stage 2/3) FLT all_lengths[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES][2][DRPTS]; FLT all_angles[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES][2][DRPTS]; int16_t all_counts[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES][2]; @@ -47,6 +47,15 @@ struct SurviveCalData int8_t found_common; int8_t times_found_common; + //For camfind (4+) + 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]; + + int senid_of_checkpt; //This is a point on a watchman that can be used to check the lh solution. + //Stage: // 0: Idle // 1: Collecting OOTX data. @@ -54,6 +63,10 @@ struct SurviveCalData }; +//The following function is not included in the core survive_cal and must be compiled from a camfind file. +//It should use data for stage 4 and report if it found the +int survive_cal_lhfind( struct SurviveCalData * cd ); + #endif diff --git a/src/survive_cal_lhfind.c b/src/survive_cal_lhfind.c new file mode 100644 index 0000000..d879f30 --- /dev/null +++ b/src/survive_cal_lhfind.c @@ -0,0 +1,22 @@ +#include "survive_cal.h" + +//Stub file for doing lhfind (this doesn't work) + +//Return +int survive_cal_lhfind( struct SurviveCalData * cd ) +{ + struct SurviveContext * ctx = cd->ctx; + + //Use the following: + // 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]; + // + // Check your solution against point: senid_of_checkpt's data. + + return -1; //Return 0 if success. +} + + -- cgit v1.2.3 From f92f5dc93cbb53a99da51984541a7e4a70605639 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 25 Feb 2017 13:29:07 -0500 Subject: Update files, working lhfind whithin libsurvive. --- src/ootx_decoder.c | 2 +- src/survive_cal.c | 27 ++--- src/survive_cal.h | 3 +- src/survive_cal_lhfind.c | 272 ++++++++++++++++++++++++++++++++++++++++++++++- src/survive_internal.h | 14 +-- 5 files changed, 294 insertions(+), 24 deletions(-) (limited to 'src') diff --git a/src/ootx_decoder.c b/src/ootx_decoder.c index 8ec16f2..e35c24d 100644 --- a/src/ootx_decoder.c +++ b/src/ootx_decoder.c @@ -112,7 +112,7 @@ void ootx_pump_bit(ootx_decoder_context *ctx, uint8_t dbit) { // printf("drop %d\n", dbit); if( !dbit ) { - printf("Bad sync bit\n"); + //printf("Bad sync bit\n"); ootx_reset_buffer(ctx); } ctx->bits_processed = 0; diff --git a/src/survive_cal.c b/src/survive_cal.c index 62cf698..760692c 100644 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -146,7 +146,7 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin int sensid = sensor_id; if( strcmp( so->codename, "WM0" ) == 0 ) sensid += 32; - if( strcmp( so->codename, "WM1" ) == 1 ) + if( strcmp( so->codename, "WM1" ) == 0 ) sensid += 64; if( sensid >= MAX_SENSORS_TO_CAL || sensid < 0 ) return; @@ -246,13 +246,15 @@ static void handle_calibration( struct SurviveCalData *cd ) #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 ) ); + memset( cd->ctsweeps, 0, sizeof( cd->ctsweeps ) ); //Either advance to stage 4 or go resetting will go back to stage 2. //What is stage 4? Are we done then? @@ -317,6 +319,7 @@ static void handle_calibration( struct SurviveCalData *cd ) if( count < DRPTS_NEEDED_FOR_AVG ) { + printf( "DPAVG %d\n", count ); //Not enough for this point to be considered. continue; } @@ -385,12 +388,12 @@ static void handle_calibration( struct SurviveCalData *cd ) 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; + int dataindex = sen*(2*NUM_LIGHTHOUSES)+lh*2+axis; + cd->avgsweeps[dataindex] = avgsweep; + cd->avglens[dataindex] = avglen; + cd->stdsweeps[dataindex] = stddevang; + cd->stdlens[dataindex] = stddevlen; + cd->ctsweeps[dataindex] = count; } fclose( hists ); fclose( ptinfo ); @@ -400,10 +403,10 @@ static void handle_calibration( struct SurviveCalData *cd ) 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]; + int ct0 = cd->ctsweeps[sen*4+0]; + int ct1 = cd->ctsweeps[sen*4+0]; + int ct2 = cd->ctsweeps[sen*4+0]; + int ct3 = cd->ctsweeps[sen*4+0]; if( ct0 > ct1 ) ct0 = ct1; if( ct0 > ct2 ) ct0 = ct2; diff --git a/src/survive_cal.h b/src/survive_cal.h index bf6161d..0a772f1 100644 --- a/src/survive_cal.h +++ b/src/survive_cal.h @@ -31,7 +31,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 ); #define MAX_SENSORS_TO_CAL 96 -#define DRPTS 512 +#define DRPTS 1024 #define MAX_CAL_PT_DAT (MAX_SENSORS_TO_CAL*NUM_LIGHTHOUSES*2) struct SurviveCalData { @@ -48,6 +48,7 @@ struct SurviveCalData int8_t times_found_common; //For camfind (4+) + //Index is calculated with: int dataindex = sen*(2*NUM_LIGHTHOUSES)+lh*2+axis; FLT avgsweeps[MAX_CAL_PT_DAT]; FLT avglens[MAX_CAL_PT_DAT]; FLT stdsweeps[MAX_CAL_PT_DAT]; diff --git a/src/survive_cal_lhfind.c b/src/survive_cal_lhfind.c index d879f30..19f732f 100644 --- a/src/survive_cal_lhfind.c +++ b/src/survive_cal_lhfind.c @@ -1,11 +1,22 @@ #include "survive_cal.h" +#include +#include +#include "linmath.h" -//Stub file for doing lhfind (this doesn't work) +#define MAX_CHECKS 40000 +#define MIN_HITS_FOR_VALID 10 + + +FLT static RunOpti( struct SurviveCalData * cd, int lh, int print, FLT * LighthousePos, FLT * LighthouseQuat ); + +//Values used for RunTest() -//Return int survive_cal_lhfind( struct SurviveCalData * cd ) { struct SurviveContext * ctx = cd->ctx; + int cycle, i; + int lh = 0; + FLT dx, dy, dz; //Use the following: // FLT avgsweeps[MAX_CAL_PT_DAT]; @@ -16,7 +27,262 @@ int survive_cal_lhfind( struct SurviveCalData * cd ) // // Check your solution against point: senid_of_checkpt's data. - return -1; //Return 0 if success. + + + for( lh = 0; lh < 2; lh++ ) + { + FLT beste = 1e20; + + FLT LighthousePos[3]; + FLT LighthouseQuat[4]; + + LighthousePos[0] = 0; + LighthousePos[1] = 0; + LighthousePos[2] = 0; + LighthouseQuat[0] = 1; + LighthouseQuat[1] = 1; + LighthouseQuat[2] = 1; + LighthouseQuat[3] = 1; + + FLT bestxyz[3]; + memcpy( bestxyz, LighthousePos, sizeof( LighthousePos ) ); + + //STAGE1 1: Detemine vectoral position from lighthouse to target. Does not determine lighthouse-target distance. + //This also is constantly optimizing the lighthouse quaternion for optimal spotting. + FLT fullrange = 5; //Maximum search space for positions. (Relative to HMD) + + + //Sweep whole area 30 times + for( cycle = 0; cycle < 30; cycle ++ ) + { + + //Adjust position, one axis at a time, over and over until we zero in. + { + FLT bestxyzrunning[3]; + beste = 1e20; + + FILE * f; + if( cycle == 0 ) + { + char filename[1024]; + sprintf( filename, "calinfo/%d_lighthouse.dat", lh ); + f = fopen( filename, "wb" ); + } + + //We split the space into this many groups (times 2) and + //if we're on the first cycle, we want to do a very linear + //search. As we refine our search we can then use a more + //binary search technique. + FLT splits = 4; + if( cycle == 0 ) splits = 32; + if( cycle == 1 ) splits = 13; + if( cycle == 2 ) splits = 10; + if( cycle == 3 ) splits = 8; + if( cycle == 4 ) splits = 5; + + //Wwe search throug the whole space. + for( dz = 0; dz < fullrange; dz += fullrange/splits ) + for( dy = -fullrange; dy < fullrange; dy += fullrange/splits ) + for( dx = -fullrange; dx < fullrange; dx += fullrange/splits ) + { + //Specificially adjust one axis at a time, searching for the best. + memcpy( LighthousePos, bestxyz, sizeof( LighthousePos ) ); + LighthousePos[0] += dx; //These are adjustments to the "best" from last frame. + LighthousePos[1] += dy; + LighthousePos[2] += dz; + + FLT ft; + //Try refining the search for the best orientation several times. + ft = RunOpti(cd, lh, 0, LighthousePos, LighthouseQuat); + if( cycle == 0 ) + { + float sk = ft*10.; + if( sk > 1 ) sk = 1; + uint8_t cell = (1.0 - sk) * 255; + FLT epsilon = 0.1; + + if( dz == 0 ) { /* Why is dz special? ? */ + if ( dx > -epsilon && dx < epsilon ) + cell = 255; + if ( dy > -epsilon && dy < epsilon ) + cell = 128; + } + + fprintf( f, "%c", cell ); + } + + if( ft < beste ) { beste = ft; memcpy( bestxyzrunning, LighthousePos, sizeof( LighthousePos ) ); } + } + + if( cycle == 0 ) + { + fclose( f ); + } + memcpy( bestxyz, bestxyzrunning, sizeof( bestxyz ) ); + + //Print out the quality of the lock this time. + FLT dist = sqrt(bestxyz[0]*bestxyz[0] + bestxyz[1]*bestxyz[1] + bestxyz[2]*bestxyz[2]); + printf( "%f %f %f (%f) = %f\n", bestxyz[0], bestxyz[1], bestxyz[2], dist, beste ); + } + + //Every cycle, tighten up the search area. + fullrange *= 0.25; + } + + if( beste > 0.005 ) + { + //Error too high + SV_ERROR( "LH: %d / Best E %f Error too high\n", lh, beste ); + return -1; + } + + cd->ctx->bsd[lh].PositionSet = 1; + copy3d( cd->ctx->bsd[lh].Position, LighthousePos ); + quatcopy( cd->ctx->bsd[lh].Quaternion, LighthouseQuat ); + } + + return 0; //Return 0 if success. } + + + + +static FLT RunOpti( struct SurviveCalData * cd, int lh, int print, FLT * LighthousePos, FLT * LighthouseQuat ) +{ + int i, p; + FLT UsToTarget[3]; + FLT LastUsToTarget[3]; + FLT mux = .9; + quatsetnone( LighthouseQuat ); + FLT * hmd_points = cd->ctx->headset.sensor_locations; + FLT * hmd_normals = cd->ctx->headset.sensor_normals; + + int first = 1, second = 0; + + //First check to see if this is a valid viewpoint. + //If a sensor is pointed away from where we are testing a possible lighthouse position. + //BUT We get data from that light house, then we KNOW this is not a possible + //lighthouse position. + for( p = 0; p < 32; p++ ) + { + int dataindex = p*(2*NUM_LIGHTHOUSES)+lh*2; + if( cd->ctsweeps[dataindex+0] < MIN_HITS_FOR_VALID || cd->ctsweeps[dataindex+1] < MIN_HITS_FOR_VALID ) continue; + FLT me_to_dot[3]; + sub3d( me_to_dot, LighthousePos, &hmd_points[p*3] ); + float dot = dot3d( &hmd_normals[p*3], me_to_dot ); + if( dot < -.01 ) { return 1000; } + } + int iters = 6; + + //Iterate over a refinement of the quaternion that constitutes the + //lighthouse. + for( i = 0; i < iters; i++ ) + { + first = 1; + for( p = 0; p < 32; p++ ) + { + int dataindex = p*(2*NUM_LIGHTHOUSES)+lh*2; + if( cd->ctsweeps[dataindex+0] < MIN_HITS_FOR_VALID || cd->ctsweeps[dataindex+1] < MIN_HITS_FOR_VALID ) continue; + + //Find out where our ray shoots forth from. + FLT ax = cd->avgsweeps[dataindex+0]; + FLT ay = cd->avgsweeps[dataindex+1]; + + //NOTE: Inputs may never be output with cross product. + //Create a fictitious normalized ray. Imagine the lighthouse is pointed + //straight in the +z direction, this is the lighthouse ray to the point. + FLT RayShootOut[3] = { sin(ax), sin(ay), 0 }; + RayShootOut[2] = sqrt( 1 - (RayShootOut[0]*RayShootOut[0] + RayShootOut[1]*RayShootOut[1]) ); + FLT RayShootOutWorld[3]; + + //Rotate that ray by the current rotation estimation. + quatrotatevector( RayShootOutWorld, LighthouseQuat, RayShootOut ); + + //Find a ray from us to the target point. + sub3d( UsToTarget, &hmd_points[p*3], LighthousePos ); + if( magnitude3d( UsToTarget ) < 0.0001 ) { continue; } + normalize3d( UsToTarget, UsToTarget ); + + FLT RotatedLastUs[3]; + quatrotatevector( RotatedLastUs, LighthouseQuat, LastUsToTarget ); + + //Rotate the lighthouse around this axis to point at the HMD. + //If it's the first time, the axis is synthesized, if it's after that, use most recent point. + FLT ConcatQuat[4]; + FLT AxisToRotate[3]; + if( first ) + { + cross3d( AxisToRotate, RayShootOutWorld, UsToTarget ); + if( magnitude3d(AxisToRotate) < 0.0001 ) break; + normalize3d( AxisToRotate, AxisToRotate ); + //Don't need to worry about being negative, cross product will fix it. + FLT RotateAmount = anglebetween3d( RayShootOutWorld, UsToTarget ); + quatfromaxisangle( ConcatQuat, AxisToRotate, RotateAmount ); + } + else + { + FLT Target[3]; + FLT Actual[3]; + + copy3d( AxisToRotate, LastUsToTarget ); + //Us to target = normalized ray from us to where we should be. + //RayShootOut = where we would be pointing. + sub3d( Target, UsToTarget, AxisToRotate ); //XXX XXX XXX WARNING THIS MESSES STUFF UP. + sub3d( Actual, RayShootOutWorld, AxisToRotate ); + if( magnitude3d( Actual ) < 0.0001 || magnitude3d( Target ) < 0.0001 ) { continue; } + normalize3d( Target, Target ); + normalize3d( Actual, Actual ); + + cross3d( AxisToRotate, Actual, Target ); //XXX Check: AxisToRotate should be equal to LastUsToTarget. + if( magnitude3d( AxisToRotate ) < 0.000001 ) { continue; } + normalize3d( AxisToRotate,AxisToRotate ); + + //printf( "%f %f %f === %f %f %f : ", PFTHREE( AxisToRotate ), PFTHREE( LastUsToTarget ) ); + FLT RotateAmount = anglebetween3d( Actual, Target ) * mux; + //printf( "FA: %f (O:%f)\n", acos( dot3d( Actual, Target ) ), RotateAmount ); + quatfromaxisangle( ConcatQuat, AxisToRotate, RotateAmount ); + } + + quatrotateabout( LighthouseQuat, ConcatQuat, LighthouseQuat ); //Chekcked. This appears to be + + mux = mux * 0.94; + if( second ) { second = 0; } + if( first ) { first = 0; second = 1; } + copy3d( LastUsToTarget, RayShootOutWorld ); + } + } + + //Step 2: Determine error. + float errorsq = 0.0; + int count = 0; + for( p = 0; p < 32; p++ ) + { + int dataindex = p*(2*NUM_LIGHTHOUSES)+lh*2; + if( cd->ctsweeps[dataindex+0] < MIN_HITS_FOR_VALID || cd->ctsweeps[dataindex+1] < MIN_HITS_FOR_VALID ) continue; + + //Find out where our ray shoots forth from. + FLT ax = cd->avgsweeps[dataindex+0]; + FLT ay = cd->avgsweeps[dataindex+1]; + FLT RayShootOut[3] = { sin(ax), sin(ay), 0 }; + RayShootOut[2] = sqrt( 1 - (RayShootOut[0]*RayShootOut[0] + RayShootOut[1]*RayShootOut[1]) ); + + //Rotate that ray by the current rotation estimation. + quatrotatevector( RayShootOut, LighthouseQuat, RayShootOut ); + + //Point-line distance. + //Line defined by LighthousePos & Direction: RayShootOut + + //Find a ray from us to the target point. + sub3d( UsToTarget, &hmd_points[p*3], LighthousePos ); + FLT xproduct[3]; + cross3d( xproduct, UsToTarget, RayShootOut ); + FLT dist = magnitude3d( xproduct ); + errorsq += dist*dist; + if( print ) printf( "%f (%d(%d/%d))\n", dist, p, cd->ctsweeps[dataindex+0], cd->ctsweeps[dataindex+1] ); + } + if( print ) printf( " = %f\n", sqrt( errorsq ) ); + return sqrt(errorsq); +} + diff --git a/src/survive_internal.h b/src/survive_internal.h index 0d0b8a4..19aa956 100644 --- a/src/survive_internal.h +++ b/src/survive_internal.h @@ -67,16 +67,16 @@ struct SurviveCalData; struct BaseStationData { uint8_t PositionSet:1; - float Position[3]; - float Quaternion[4]; + FLT Position[3]; + FLT Quaternion[4]; uint8_t OOTXSet:1; uint32_t BaseStationID; - float fcalphase[2]; - float fcaltilt[2]; - float fcalcurve[2]; - float fcalgibpha[2]; - float fcalgibmag[2]; + FLT fcalphase[2]; + FLT fcaltilt[2]; + FLT fcalcurve[2]; + FLT fcalgibpha[2]; + FLT fcalgibmag[2]; }; struct SurviveContext -- cgit v1.2.3 From 4c4969d0cb0f817de2aa866a7d8abfde6ecaff82 Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Sat, 25 Feb 2017 14:20:28 -0500 Subject: initial commit of config parser --- src/survive_config.c | 182 +++++++++++++++++++++++++++++++++++++++++++++++++++ src/survive_config.h | 36 ++++++++++ 2 files changed, 218 insertions(+) create mode 100644 src/survive_config.c create mode 100644 src/survive_config.h (limited to 'src') diff --git a/src/survive_config.c b/src/survive_config.c new file mode 100644 index 0000000..42c2c89 --- /dev/null +++ b/src/survive_config.c @@ -0,0 +1,182 @@ +#include +#include +#include "survive_config.h" + +#define MAX_CONFIG_ENTRIES 100 + +config_val config_values[MAX_CONFIG_ENTRIES]; +static uint16_t used_entries = 0; + +static FILE *config_file = NULL; + +void config_init() { + uint16_t i = 0; + for (i=0;i< Joshua Allen, Under MIT/x11 License. + + +#ifndef _SURVIVE_CONFIG_H +#define _SURVIVE_CONFIG_H + +#include "survive_internal.h" + +typedef enum { + CONFIG_UNKNOWN = 0, + CONFIG_FLOAT = 1, + CONFIG_UINT32 = 2, + CONFIG_STRING = 3 +} cval_type; +/* +typedef union { + uint32_t i; + float f; + } Numeric; +*/ +typedef struct { + char *tag; + cval_type type; + union { + uint32_t i; + float f; + } numeric; + char *str; +} config_val; + + +void config_open(const char* path, const char* mode); +void config_close(); +void config_write_lighthouse(struct BaseStationData* bsd, uint8_t length); + +#endif \ No newline at end of file -- cgit v1.2.3 From 3baabd2f8aa69a986bbb29739e6353d9a95336c5 Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Sat, 25 Feb 2017 14:34:10 -0500 Subject: float to FLT --- src/survive_config.c | 20 ++++++-------------- src/survive_config.h | 13 +++++++++++-- 2 files changed, 17 insertions(+), 16 deletions(-) (limited to 'src') diff --git a/src/survive_config.c b/src/survive_config.c index 42c2c89..254e52f 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -19,11 +19,11 @@ void config_init() { used_entries = 0; } -void write_float(char* tag, float x) { +void write_float(char* tag, FLT x) { fprintf(config_file, "\"%s\":\"%f\"\n", tag, x); } -void write_float_a(char* tag, float *x, uint8_t count) { +void write_float_a(char* tag, FLT *x, uint8_t count) { uint8_t i = 0; char idx[4]; for (i=0;i Date: Sat, 25 Feb 2017 15:14:10 -0500 Subject: integer not float --- src/survive_config.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/survive_config.c b/src/survive_config.c index 254e52f..ed3f6cd 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -163,7 +163,7 @@ void config_save(const char* path) { if (config_values[i].type == CONFIG_FLOAT) { fprintf(f, "\"%s\":\"%F\"\n", config_values[i].tag, config_values[i].numeric.f); } else if (config_values[i].type == CONFIG_UINT32) { - fprintf(f, "\"%s\":\"%F\"\n", config_values[i].tag, config_values[i].numeric.i); + fprintf(f, "\"%s\":\"%d\"\n", config_values[i].tag, config_values[i].numeric.i); } else if (config_values[i].type == CONFIG_STRING) { fprintf(f, "\"%s\":\"%s\"\n", config_values[i].tag, config_values[i].str); } -- cgit v1.2.3 From c2ee02ce28a7c7a9bebe9b69f214e8e441d60cc8 Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Sat, 25 Feb 2017 16:18:26 -0500 Subject: specify types --- src/survive_config.c | 9 +++++++++ 1 file changed, 9 insertions(+) (limited to 'src') diff --git a/src/survive_config.c b/src/survive_config.c index ed3f6cd..09153fa 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -1,3 +1,5 @@ +// (C) 2017 <>< Joshua Allen, Under MIT/x11 License. + #include #include #include "survive_config.h" @@ -14,6 +16,7 @@ void config_init() { for (i=0;i Date: Sat, 25 Feb 2017 16:20:42 -0500 Subject: Update code to allow for headless mode. --- src/survive.c | 33 +++++++++++++++++++-------------- 1 file changed, 19 insertions(+), 14 deletions(-) (limited to 'src') diff --git a/src/survive.c b/src/survive.c index aab2ae6..473a26d 100644 --- a/src/survive.c +++ b/src/survive.c @@ -134,7 +134,7 @@ static int LoadConfig( struct SurviveContext * ctx, struct SurviveObject * so, i return 0; } -struct SurviveContext * survive_init() +struct SurviveContext * survive_init( int headless ) { int r = 0; struct SurviveContext * ctx = calloc( 1, sizeof( struct SurviveContext ) ); @@ -153,17 +153,20 @@ struct SurviveContext * survive_init() ctx->watchman[1].ctx = ctx; memcpy( ctx->watchman[1].codename, "WM1", 4 ); - //USB must happen last. - if( r = survive_usb_init( ctx ) ) - { - //TODO: Cleanup any libUSB stuff sitting around. - goto fail_gracefully; - } - - //Next, pull out the config stuff. - if( LoadConfig( ctx, &ctx->headset, 1, 0, 0 ) ) goto fail_gracefully; - if( LoadConfig( ctx, &ctx->watchman[0], 2, 0, 1 ) ) { SV_INFO( "Watchman 0 config issue." ); } - if( LoadConfig( ctx, &ctx->watchman[1], 3, 0, 1 ) ) { SV_INFO( "Watchman 1 config issue." ); } + if( !headless ) + { + //USB must happen last. + if( r = survive_usb_init( ctx ) ) + { + //TODO: Cleanup any libUSB stuff sitting around. + goto fail_gracefully; + } + + //Next, pull out the config stuff. + if( LoadConfig( ctx, &ctx->headset, 1, 0, 0 ) ) goto fail_gracefully; + if( LoadConfig( ctx, &ctx->watchman[0], 2, 0, 1 ) ) { SV_INFO( "Watchman 0 config issue." ); } + if( LoadConfig( ctx, &ctx->watchman[1], 3, 0, 1 ) ) { SV_INFO( "Watchman 1 config issue." ); } + } ctx->headset.timebase_hz = ctx->watchman[0].timebase_hz = ctx->watchman[1].timebase_hz = 48000000; ctx->headset.pulsedist_max_ticks = ctx->watchman[0].pulsedist_max_ticks = ctx->watchman[1].pulsedist_max_ticks = 500000; @@ -253,12 +256,14 @@ void survive_install_angle_fn( struct SurviveContext * ctx, angle_process_func void survive_close( struct SurviveContext * ctx ) { - survive_usb_close( ctx ); + if( ctx->usbctx ) + survive_usb_close( ctx ); } int survive_poll( struct SurviveContext * ctx ) { - survive_usb_poll( ctx ); + if( ctx->usbctx ) + survive_usb_poll( ctx ); } int survive_simple_inflate( struct SurviveContext * ctx, const char * input, int inlen, char * output, int outlen ) -- cgit v1.2.3 From 55cedfc6a6b035d6eb54457782818fef61cae500 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 25 Feb 2017 23:52:48 -0500 Subject: Huge shift: Put HTC vive into its own file, to free up the rest of the system for libsurvive. --- src/survive.c | 249 ++++------- src/survive_cal.c | 8 + src/survive_cal.h | 2 + src/survive_cal_lhfind.c | 5 +- src/survive_data.c | 358 +-------------- src/survive_driverman.c | 50 +++ src/survive_driverman.h | 22 + src/survive_internal.h | 65 +-- src/survive_usb.c | 433 +------------------ src/survive_vive.c | 1075 ++++++++++++++++++++++++++++++++++++++++++++++ 10 files changed, 1259 insertions(+), 1008 deletions(-) create mode 100644 src/survive_driverman.c create mode 100644 src/survive_driverman.h create mode 100644 src/survive_vive.c (limited to 'src') diff --git a/src/survive.c b/src/survive.c index aab2ae6..b472ccc 100644 --- a/src/survive.c +++ b/src/survive.c @@ -3,22 +3,12 @@ #include #include "survive_internal.h" +#include "survive_driverman.h" #include #include -#include #include #include - -static int jsoneq(const char *json, jsmntok_t *tok, const char *s) { - if (tok->type == JSMN_STRING && (int) strlen(s) == tok->end - tok->start && - strncmp(json + tok->start, s, tok->end - tok->start) == 0) { - return 0; - } - return -1; -} - - static void survivefault( struct SurviveContext * ctx, const char * fault ) { fprintf( stderr, "Error: %s\n", fault ); @@ -30,113 +20,11 @@ static void survivenote( struct SurviveContext * ctx, const char * fault ) fprintf( stderr, "Info: %s\n", fault ); } -static int ParsePoints( struct SurviveContext * ctx, struct SurviveObject * so, char * ct0conf, FLT ** floats_out, jsmntok_t * t, int i ) -{ - int k; - int pts = t[i+1].size; - jsmntok_t * tk; - - so->nr_locations = 0; - *floats_out = malloc( sizeof( **floats_out ) * 32 * 3 ); - - for( k = 0; k < pts; k++ ) - { - tk = &t[i+2+k*4]; - - FLT vals[3]; - int m; - for( m = 0; m < 3; m++ ) - { - char ctt[128]; - - tk++; - int elemlen = tk->end - tk->start; - - if( tk->type != 4 || elemlen > sizeof( ctt )-1 ) - { - SV_ERROR( "Parse error in JSON\n" ); - return 1; - } - - memcpy( ctt, ct0conf + tk->start, elemlen ); - ctt[elemlen] = 0; - FLT f = atof( ctt ); - int id = so->nr_locations*3+m; - (*floats_out)[id] = f; - } - so->nr_locations++; - } - return 0; -} - -static int LoadConfig( struct SurviveContext * ctx, struct SurviveObject * so, int devno, int iface, int extra_magic ) -{ - char * ct0conf = 0; - int len = survive_get_config( &ct0conf, ctx, devno, iface, extra_magic ); - -#if 0 - char fname[100]; - sprintf( fname, "%s_config.json", so->codename ); - FILE * f = fopen( fname, "w" ); - fwrite( ct0conf, strlen(ct0conf), 1, f ); - fclose( f ); -#endif - - if( len > 0 ) - { - - //From JSMN example. - jsmn_parser p; - jsmntok_t t[4096]; - jsmn_init(&p); - int i; - int r = jsmn_parse(&p, ct0conf, len, t, sizeof(t)/sizeof(t[0])); - if (r < 0) { - SV_INFO("Failed to parse JSON in HMD configuration: %d\n", r); - return -1; - } - if (r < 1 || t[0].type != JSMN_OBJECT) { - SV_INFO("Object expected in HMD configuration\n"); - return -2; - } - - for (i = 1; i < r; i++) { - jsmntok_t * tk = &t[i]; - - char ctxo[100]; - int ilen = tk->end - tk->start; - if( ilen > 99 ) ilen = 99; - memcpy(ctxo, ct0conf + tk->start, ilen); - ctxo[ilen] = 0; - -// printf( "%d / %d / %d / %d %s %d\n", tk->type, tk->start, tk->end, tk->size, ctxo, jsoneq(ct0conf, &t[i], "modelPoints") ); -// printf( "%.*s\n", ilen, ct0conf + tk->start ); - - if (jsoneq(ct0conf, tk, "modelPoints") == 0) { - if( ParsePoints( ctx, so, ct0conf, &so->sensor_locations, t, i ) ) - { - break; - } - } - if (jsoneq(ct0conf, tk, "modelNormals") == 0) { - if( ParsePoints( ctx, so, ct0conf, &so->sensor_normals, t, i ) ) - { - break; - } - } - } - } - else - { - //TODO: Cleanup any remaining USB stuff. - return 1; - } - return 0; -} struct SurviveContext * survive_init() { int r = 0; + int i = 0; struct SurviveContext * ctx = calloc( 1, sizeof( struct SurviveContext ) ); ctx->faultfunction = survivefault; @@ -146,66 +34,16 @@ struct SurviveContext * survive_init() ctx->imuproc = survive_default_imu_process; ctx->angleproc = survive_default_angle_process; - ctx->headset.ctx = ctx; - memcpy( ctx->headset.codename, "HMD", 4 ); - ctx->watchman[0].ctx = ctx; - memcpy( ctx->watchman[0].codename, "WM0", 4 ); - ctx->watchman[1].ctx = ctx; - memcpy( ctx->watchman[1].codename, "WM1", 4 ); - - //USB must happen last. - if( r = survive_usb_init( ctx ) ) - { - //TODO: Cleanup any libUSB stuff sitting around. - goto fail_gracefully; - } - - //Next, pull out the config stuff. - if( LoadConfig( ctx, &ctx->headset, 1, 0, 0 ) ) goto fail_gracefully; - if( LoadConfig( ctx, &ctx->watchman[0], 2, 0, 1 ) ) { SV_INFO( "Watchman 0 config issue." ); } - if( LoadConfig( ctx, &ctx->watchman[1], 3, 0, 1 ) ) { SV_INFO( "Watchman 1 config issue." ); } - - ctx->headset.timebase_hz = ctx->watchman[0].timebase_hz = ctx->watchman[1].timebase_hz = 48000000; - ctx->headset.pulsedist_max_ticks = ctx->watchman[0].pulsedist_max_ticks = ctx->watchman[1].pulsedist_max_ticks = 500000; - ctx->headset.pulselength_min_sync = ctx->watchman[0].pulselength_min_sync = ctx->watchman[1].pulselength_min_sync = 2200; - ctx->headset.pulse_in_clear_time = ctx->watchman[0].pulse_in_clear_time = ctx->watchman[1].pulse_in_clear_time = 35000; - ctx->headset.pulse_max_for_sweep = ctx->watchman[0].pulse_max_for_sweep = ctx->watchman[1].pulse_max_for_sweep = 1800; - - ctx->headset.pulse_synctime_offset = ctx->watchman[0].pulse_synctime_offset = ctx->watchman[1].pulse_synctime_offset = 20000; - ctx->headset.pulse_synctime_slack = ctx->watchman[0].pulse_synctime_slack = ctx->watchman[1].pulse_synctime_slack = 5000; - - ctx->headset.timecenter_ticks = ctx->headset.timebase_hz / 240; - ctx->watchman[0].timecenter_ticks = ctx->watchman[0].timebase_hz / 240; - ctx->watchman[1].timecenter_ticks = ctx->watchman[1].timebase_hz / 240; -/* - int i; - int locs = ctx->headset.nr_locations; - printf( "Locs: %d\n", locs ); - if (ctx->headset.sensor_locations ) + const char * DriverName; + while( ( DriverName = GetDriverNameMatching( "DriverReg", i++ ) ) ) { - printf( "POSITIONS:\n" ); - for( i = 0; i < locs*3; i+=3 ) - { - printf( "%f %f %f\n", ctx->headset.sensor_locations[i+0], ctx->headset.sensor_locations[i+1], ctx->headset.sensor_locations[i+2] ); - } + DeviceDriver dd = GetDriver( DriverName ); + printf( "Loading driver %s (%p) (%d)\n", DriverName, dd, i ); + r = dd( ctx ); + printf( "Driver %s reports status %d\n", DriverName, r ); } - if( ctx->headset.sensor_normals ) - { - printf( "NORMALS:\n" ); - for( i = 0; i < locs*3; i+=3 ) - { - printf( "%f %f %f\n", ctx->headset.sensor_normals[i+0], ctx->headset.sensor_normals[i+1], ctx->headset.sensor_normals[i+2] ); - } - } -*/ - - return ctx; -fail_gracefully: - survive_usb_close( ctx ); - free( ctx ); - return 0; } void survive_install_info_fn( struct SurviveContext * ctx, text_feedback_func fbp ) @@ -249,18 +87,76 @@ void survive_install_angle_fn( struct SurviveContext * ctx, angle_process_func ctx->angleproc = survive_default_angle_process; } +int survive_add_object( struct SurviveContext * ctx, struct SurviveObject * obj ) +{ + int oldct = ctx->objs_ct; + ctx->objs = realloc( ctx->objs, sizeof( struct SurviveObject * ) * (oldct+1) ); + ctx->objs[oldct] = obj; + ctx->objs_ct = oldct+1; +} + +void survive_add_driver( struct SurviveContext * ctx, void * payload, DeviceDriverCb poll, DeviceDriverCb close, DeviceDriverMagicCb magic ) +{ + int oldct = ctx->driver_ct; + ctx->drivers = realloc( ctx->drivers, sizeof( void * ) * (oldct+1) ); + ctx->driverpolls = realloc( ctx->driverpolls, sizeof( DeviceDriverCb * ) * (oldct+1) ); + ctx->drivercloses = realloc( ctx->drivercloses, sizeof( DeviceDriverCb * ) * (oldct+1) ); + ctx->drivermagics = realloc( ctx->drivermagics, sizeof( DeviceDriverMagicCb * ) * (oldct+1) ); + ctx->drivers[oldct] = payload; + ctx->driverpolls[oldct] = poll; + ctx->drivercloses[oldct] = close; + ctx->drivermagics[oldct] = magic; + ctx->driver_ct = oldct+1; +} +int survive_send_magic( struct SurviveContext * ctx, int magic_code, void * data, int datalen ) +{ + int oldct = ctx->driver_ct; + int i; + for( i = 0; i < oldct; i++ ) + { + ctx->drivermagics[i]( ctx, ctx->drivers[i], magic_code, data, datalen ); + } +} void survive_close( struct SurviveContext * ctx ) { - survive_usb_close( ctx ); + const char * DriverName; + int r = 0; + while( ( DriverName = GetDriverNameMatching( "DriverUnreg", r++ ) ) ) + { + DeviceDriver dd = GetDriver( DriverName ); + SV_INFO( "De-registering driver %s (%p)", DriverName, dd ); + r = dd( ctx ); + SV_INFO( "Driver %s reports status %d", DriverName, r ); + } + + int oldct = ctx->driver_ct; + int i; + for( i = 0; i < oldct; i++ ) + { + ctx->driverpolls[i]( ctx, ctx->drivers[i] ); + } + + //TODO: Free everything except for self. + //XXX Will leak memory. } int survive_poll( struct SurviveContext * ctx ) { - survive_usb_poll( ctx ); + int oldct = ctx->driver_ct; + int i, r; + + for( i = 0; i < oldct; i++ ) + { + r = ctx->driverpolls[i]( ctx, ctx->drivers[i] ); + if( r ) return r; + } + + return 0; } + int survive_simple_inflate( struct SurviveContext * ctx, const char * input, int inlen, char * output, int outlen ) { z_stream zs; //Zlib stream. May only be used by configuration at beginning and by USB thread periodically. @@ -285,9 +181,12 @@ int survive_simple_inflate( struct SurviveContext * ctx, const char * input, int struct SurviveObject * survive_get_so_by_name( struct SurviveContext * ctx, const char * name ) { - if( strcmp( name, "HMD" ) == 0 ) return &ctx->headset; - if( strcmp( name, "WM0" ) == 0 ) return &ctx->watchman[0]; - if( strcmp( name, "WM1" ) == 0 ) return &ctx->watchman[1]; + int i; + for( i = 0; i < ctx->objs_ct; i++ ) + { + if( strcmp( ctx->objs[i]->codename, name ) == 0 ) + return ctx->objs[i]; + } return 0; } diff --git a/src/survive_cal.c b/src/survive_cal.c index 760692c..2ed1986 100644 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -96,6 +96,14 @@ void survive_cal_install( struct SurviveContext * ctx ) cd->stage = 1; cd->ctx = ctx; + cd->hmd = survive_get_so_by_name( ctx, "HMD" ); + if( !cd->hmd ) + { + SV_ERROR( "Error: cannot find any devices labeled HMD. Required for calibration" ); + free( cd ); + return; + } + ootx_packet_clbk = ootx_packet_clbk_d; ctx->calptr = cd; diff --git a/src/survive_cal.h b/src/survive_cal.h index 0a772f1..bb4eb32 100644 --- a/src/survive_cal.h +++ b/src/survive_cal.h @@ -57,6 +57,8 @@ struct SurviveCalData int senid_of_checkpt; //This is a point on a watchman that can be used to check the lh solution. + struct SurviveObject * hmd; + //Stage: // 0: Idle // 1: Collecting OOTX data. diff --git a/src/survive_cal_lhfind.c b/src/survive_cal_lhfind.c index 19f732f..e1e5fc9 100644 --- a/src/survive_cal_lhfind.c +++ b/src/survive_cal_lhfind.c @@ -156,8 +156,9 @@ static FLT RunOpti( struct SurviveCalData * cd, int lh, int print, FLT * Lightho FLT LastUsToTarget[3]; FLT mux = .9; quatsetnone( LighthouseQuat ); - FLT * hmd_points = cd->ctx->headset.sensor_locations; - FLT * hmd_normals = cd->ctx->headset.sensor_normals; + struct SurviveObject * hmd = cd->hmd; + FLT * hmd_points = hmd->sensor_locations; + FLT * hmd_normals = hmd->sensor_normals; int first = 1, second = 0; diff --git a/src/survive_data.c b/src/survive_data.c index 402282d..e55b640 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -1,35 +1,12 @@ //<>< (C) 2016 C. N. Lohr, MOSTLY Under MIT/x11 License. // -//Based off of https://github.com/collabora/OSVR-Vive-Libre -// Originally Copyright 2016 Philipp Zabel -// Originally Copyright 2016 Lubosz Sarnecki -// Originally Copyright (C) 2013 Fredrik Hultin -// Originally Copyright (C) 2013 Jakob Bornecrantz -// -//But, re-written as best as I can to get it put under an open souce license instead of a forced-source license. -//If there are portions of the code too similar to the original, I would like to know so they can be re-written. -//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses. #include "survive_internal.h" #include #include -#define POP1 (*(readdata++)) -#define POP2 (*(((uint16_t*)((readdata+=2)-2)))) -#define POP4 (*(((uint32_t*)((readdata+=4)-4)))) - - -struct LightcapElement -{ - uint8_t sensor_id; - uint8_t type; - uint16_t length; - uint32_t timestamp; -} __attribute__((packed)); - - //This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode. -static void handle_lightcap( struct SurviveObject * so, struct LightcapElement * le ) +void handle_lightcap( struct SurviveObject * so, struct LightcapElement * le ) { struct SurviveContext * ctx = so->ctx; //int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time; @@ -200,336 +177,3 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement * } - -static void handle_watchman( struct SurviveObject * w, uint8_t * readdata ) -{ - int i; - - uint8_t startread[29]; - memcpy( startread, readdata, 29 ); - -#if 0 - printf( "DAT: " ); - for( i = 0; i < 29; i++ ) - { - printf( "%02x ", readdata[i] ); - } - printf("\n"); -#endif - - uint8_t time1 = POP1; - uint8_t qty = POP1; - uint8_t time2 = POP1; - uint8_t type = POP1; - qty-=2; - int propset = 0; - int doimu = 0; - - - if( (type & 0xf0) == 0xf0 ) - { - propset |= 4; - //printf( "%02x %02x %02x %02x\n", qty, type, time1, time2 ); - type &= ~0x10; - - if( type & 0x01 ) - { - qty-=1; - w->buttonmask = POP1; - type &= ~0x01; - } - if( type & 0x04 ) - { - qty-=1; - w->axis1 = ( POP1 ) * 128; - type &= ~0x04; - } - if( type & 0x02 ) - { - qty-=4; - w->axis2 = POP2; - w->axis3 = POP2; - type &= ~0x02; - } - - //XXX TODO: Is this correct? It looks SO WACKY - type &= 0x7f; - if( type == 0x68 ) doimu = 1; - type &= 0x0f; - if( type == 0x00 && qty ) { type = POP1; qty--; } - } - - if( type == 0xe1 ) - { - propset |= 1; - w->charging = readdata[0]>>7; - w->charge = POP1&0x7f; qty--; - w->ison = 1; - if( qty ) - { - qty--; - type = POP1; //IMU usually follows. - } - } - - if( ( ( type & 0xe8 ) == 0xe8 ) || doimu ) //Hmm, this looks kind of yucky... we can get e8's that are accelgyro's but, cleared by first propset. - { - propset |= 2; - w->ctx->imuproc( w, (int16_t *)&readdata[1], (time1<<24)|(time2<<16)|readdata[0], 0 ); - int16_t * k = (int16_t *)readdata+1; - //printf( "Match8 %d %d %d %d %d %3d %3d\n", qty, k[0], k[1], k[2], k[3], k[4], k[5] ); - readdata += 13; qty -= 13; - type &= ~0xe8; - if( qty ) - { - qty--; - type = POP1; - } - } - - - if( qty ) - { - int j; - qty++; - readdata--; - *readdata = type; //Put 'type' back on stack. - uint8_t * mptr = readdata + qty-3-1; //-3 for timecode, -1 to - -//#define DEBUG_WATCHMAN -#ifdef DEBUG_WATCHMAN - printf( "_%s ", w->codename); - for( i = 0; i < qty; i++ ) - { - printf( "%02x ", readdata[i] ); - } - printf("\n"); -#endif - - - uint32_t mytime = (mptr[3] << 16)|(mptr[2] << 8)|(mptr[1] << 0); - - uint32_t times[20]; - const int nrtime = sizeof(times)/sizeof(uint32_t); - int timecount = 0; - int leds; - int parameters; - int fault = 0; - - ///Handle uint32_tifying (making sure we keep it incrementing) - uint32_t llt = w->last_lighttime; - uint32_t imumsb = time1<<24; - mytime |= imumsb; - - //Compare mytime to llt - - int diff = mytime - llt; - if( diff < -0x1000000 ) - mytime += 0x1000000; - else if( diff > 0x100000 ) - mytime -= 0x1000000; - - w->last_lighttime = mytime; - - times[timecount++] = mytime; -#ifdef DEBUG_WATCHMAN - printf( "_%s Packet Start Time: %d\n", w->codename, mytime ); -#endif - - //First, pull off the times, starting with the current time, then all the delta times going backwards. - { - while( mptr - readdata > (timecount>>1) ) - { - uint32_t arcane_value = 0; - //ArcanePop (Pop off values from the back, forward, checking if the MSB is set) - do - { - uint8_t ap = *(mptr--); - arcane_value |= (ap&0x7f); - if( ap & 0x80 ) break; - arcane_value <<= 7; - } while(1); - times[timecount++] = (mytime -= arcane_value); -#ifdef DEBUG_WATCHMAN - printf( "_%s Time: %d newtime: %d\n", w->codename, arcane_value, mytime ); -#endif - } - - leds = timecount>>1; - //Check that the # of sensors at the beginning match the # of parameters we would expect. - if( timecount & 1 ) { fault = 1; goto end; } //Inordinal LED count - if( leds != mptr - readdata + 1 ) { fault = 2; goto end; } //LED Count does not line up with parameters - } - - - struct LightcapElement les[10]; - int lese = 0; //les's end - - //Second, go through all LEDs and extract the lightevent from them. - { - uint8_t marked[nrtime]; - memset( marked, 0, sizeof( marked ) ); - int i, parpl = 0; - timecount--; - int timepl = 0; - - //This works, but usually returns the values in reverse end-time order. - for( i = 0; i < leds; i++ ) - { - int led = readdata[i]; - int adv = led & 0x07; - led >>= 3; - - while( marked[timepl] ) timepl++; - if( timepl > timecount ) { fault = 3; goto end; } //Ran off max of list. - uint32_t endtime = times[timepl++]; - int end = timepl + adv; - if( end > timecount ) { fault = 4; goto end; } //end referencing off list - if( marked[end] > 0 ) { fault = 5; goto end; } //Already marked trying to be used. - uint32_t starttime = times[end]; - marked[end] = 1; - - //Insert all lighting things into a sorted list. This list will be - //reverse sorted, but that is to minimize operations. To read it - //in sorted order simply read it back backwards. - //Use insertion sort, since we should most of the time, be in order. - struct LightcapElement * le = &les[lese++]; - le->sensor_id = led; - le->type = 0xfe; - - if( (uint32_t)(endtime - starttime) > 65535 ) { fault = 6; goto end; } //Length of pulse dumb. - le->length = endtime - starttime; - le->timestamp = starttime; - -#ifdef DEBUG_WATCHMAN - printf( "_%s Event: %d %d %d-%d\n", w->codename, led, le->length, endtime, starttime ); -#endif - int swap = lese-2; - while( swap >= 0 && les[swap].timestamp < les[swap+1].timestamp ) - { - struct LightcapElement l; - memcpy( &l, &les[swap], sizeof( l ) ); - memcpy( &les[swap], &les[swap+1], sizeof( l ) ); - memcpy( &les[swap+1], &l, sizeof( l ) ); - swap--; - } - } - } - - int i; - for( i = lese-1; i >= 0; i-- ) - { - //printf( "%d: %d [%d]\n", les[i].sensor_id, les[i].length, les[i].timestamp ); - handle_lightcap( w, &les[i] ); - } - - return; - end: - { - struct SurviveContext * ctx = w->ctx; - SV_INFO( "Light decoding fault: %d\n", fault ); - } - } -} - - -void survive_data_cb( struct SurviveUSBInterface * si ) -{ - int size = si->actual_len; - struct SurviveContext * ctx = si->ctx; -#if 0 - int i; - printf( "%16s: %d: ", si->hname, len ); - for( i = 0; i < size; i++ ) - { - printf( "%02x ", si->buffer[i] ); - } - printf( "\n" ); - return; -#endif - - int iface = si->which_interface_am_i; - uint8_t * readdata = si->buffer; - - int id = POP1; -// printf( "%16s Size: %2d ID: %d / %d\n", si->hname, size, id, iface ); - - - switch( si->which_interface_am_i ) - { - case USB_IF_HMD: - { - struct SurviveObject * headset = &ctx->headset; - readdata+=2; - headset->buttonmask = POP1; //Lens - headset->axis2 = POP2; //Lens Separation - readdata+=2; - headset->buttonmask |= POP1; //Button - readdata+=3; - readdata++; //Proxchange, No change = 0, Decrease = 1, Increase = 2 - readdata++; - headset->axis3 = POP2; //Proximity << how close to face are you? Less than 80 = not on face. - headset->axis1 = POP2; //IPD << what is this? - headset->ison = 1; - break; - } - case USB_IF_LIGHTHOUSE: - { - int i; - //printf( "%d -> ", size ); - for( i = 0; i < 3; i++ ) - { - int16_t * acceldata = (int16_t*)readdata; - readdata += 12; - uint32_t timecode = POP4; - uint8_t code = POP1; - //printf( "%d ", code ); - int8_t cd = code - ctx->headset.oldcode; - - if( cd > 0 ) - { - ctx->headset.oldcode = code; - ctx->imuproc( &ctx->headset, acceldata, timecode, code ); - } - } - - //DONE OK. - break; - } - case USB_IF_WATCHMAN1: - case USB_IF_WATCHMAN2: - { - struct SurviveObject * w = &ctx->watchman[si->which_interface_am_i-USB_IF_WATCHMAN1]; - if( id == 35 ) - { - handle_watchman( w, readdata); - } - else if( id == 36 ) - { - handle_watchman( w, readdata); - handle_watchman( w, readdata+29 ); - } - else if( id == 38 ) - { - w->ison = 0; - } - else - { - SV_INFO( "Unknown watchman code %d\n", id ); - } - break; - } - case USB_IF_LIGHTCAP: - { - //Done! - int i; - for( i = 0; i < 7; i++ ) - { - handle_lightcap( &ctx->headset, (struct LightcapElement*)&readdata[i*8] ); - } - break; - } - } -} - - diff --git a/src/survive_driverman.c b/src/survive_driverman.c new file mode 100644 index 0000000..b4684c8 --- /dev/null +++ b/src/survive_driverman.c @@ -0,0 +1,50 @@ +#include "survive_driverman.h" +#include +#include + +static void * Drivers[MAX_DRIVERS]; +static const char * DriverNames[MAX_DRIVERS]; +static int NrDrivers; + +void RegisterDriver( const char * element, void * data ) +{ + Drivers[NrDrivers] = data; + DriverNames[NrDrivers] = element; + NrDrivers++; +} + +void * GetDriver( const char * element ) +{ + int i; + for( i = 0; i < NrDrivers; i++ ) + { + if( strcmp( element, DriverNames[i] ) == 0 ) return Drivers[i]; + } + return 0; +} + +const char * GetDriverNameMatching( const char * prefix, int place ) +{ + int i; + int prefixlen = strlen( prefix ); + + for( i = 0; i < NrDrivers; i++ ) + { + if( memcmp( prefix, DriverNames[i], prefixlen ) == 0 ) + if( 0 == (place--) ) + return DriverNames[i]; + } + return 0; +} + +void ListDrivers() +{ + int i; + printf( "Drivers (%d/%d):\n", NrDrivers, MAX_DRIVERS ); + for( i = 0; i < NrDrivers; i++ ) + { + printf( " %s\n", DriverNames[i] ); + } + +} + diff --git a/src/survive_driverman.h b/src/survive_driverman.h new file mode 100644 index 0000000..fb385da --- /dev/null +++ b/src/survive_driverman.h @@ -0,0 +1,22 @@ +#ifndef SURVIVE_DRIVERMAN_H +#define SURVIVE_DRIVERMAN_H + +//Driver registration +#define MAX_DRIVERS 32 + +void RegisterDriver( const char * name, void * data ); +void * GetDriver( const char * name ); +const char * GetDriverNameMatching( const char * prefix, int place ); +void ListDrivers(); + +#define REGISTER_LINKTIME( func ) \ + void __attribute__((constructor)) Register##func() { RegisterDriver( #func, &func ); } + +struct SurviveContext; + +typedef int (*DeviceDriver)( struct SurviveContext * ctx ); +typedef int (*DeviceDriverCb)( struct SurviveContext * ctx, void * driver ); +typedef int (*DeviceDriverMagicCb)( struct SurviveContext * ctx, void * driver, int magic_code, void * data, int datalen ); + +#endif + diff --git a/src/survive_internal.h b/src/survive_internal.h index 19aa956..3dc471a 100644 --- a/src/survive_internal.h +++ b/src/survive_internal.h @@ -17,7 +17,7 @@ #include #include #include -#include +#include "survive_driverman.h" #include #include @@ -27,39 +27,12 @@ //XXX TODO This one needs to be rewritten. #define SV_KILL() exit(0) -#define USB_DEV_HMD 0 -#define USB_DEV_LIGHTHOUSE 1 -#define USB_DEV_WATCHMAN1 2 -#define USB_DEV_WATCHMAN2 3 -#define MAX_USB_DEVS 4 - - -#define USB_IF_HMD 0 -#define USB_IF_LIGHTHOUSE 1 -#define USB_IF_WATCHMAN1 2 -#define USB_IF_WATCHMAN2 3 -#define USB_IF_LIGHTCAP 4 -#define MAX_INTERFACES 5 - #define INTBUFFSIZE 64 #define SENSORS_PER_OBJECT 32 struct SurviveContext; struct SurviveUSBInterface; -typedef void (*usb_callback)( struct SurviveUSBInterface * ti ); - -struct SurviveUSBInterface -{ - struct libusb_transfer * transfer; - struct SurviveContext * ctx; - int actual_len; - uint8_t buffer[INTBUFFSIZE]; - usb_callback cb; - int which_interface_am_i; //for indexing into uiface - const char * hname; //human-readable names -}; - //This is defined in survive.h struct SurviveObject; struct SurviveCalData; @@ -81,35 +54,43 @@ struct BaseStationData struct SurviveContext { - //USB Subsystem - struct libusb_context* usbctx; - struct libusb_device_handle * udev[MAX_USB_DEVS]; - struct SurviveUSBInterface uiface[MAX_INTERFACES]; - text_feedback_func faultfunction; text_feedback_func notefunction; light_process_func lightproc; imu_process_func imuproc; angle_process_func angleproc; - //Calibration data: struct BaseStationData bsd[NUM_LIGHTHOUSES]; struct SurviveCalData * calptr; //If and only if the calibration subsystem is attached. - //Data Subsystem. These should be last, as there may be additional surviveobjects. - struct SurviveObject headset; - struct SurviveObject watchman[2]; //Currently only two supported watchmen. + struct SurviveObject ** objs; + int objs_ct; + void ** drivers; + DeviceDriverCb * driverpolls; + DeviceDriverCb * drivercloses; + DeviceDriverMagicCb * drivermagics; + int driver_ct; }; +int survive_add_object( struct SurviveContext * ctx, struct SurviveObject * obj ); + +void survive_add_driver( struct SurviveContext * ctx, void * payload, DeviceDriverCb poll, DeviceDriverCb close, DeviceDriverMagicCb magic ); + +//For lightcap, etc. Don't change this structure at all. Regular vive is dependent on it being exactly as-is. +struct LightcapElement +{ + uint8_t sensor_id; + uint8_t type; + uint16_t length; + uint32_t timestamp; +} __attribute__((packed)); + +//This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode. +void handle_lightcap( struct SurviveObject * so, struct LightcapElement * le ); -//USB Subsystem -void survive_usb_close( struct SurviveContext * t ); -int survive_usb_init( struct SurviveContext * t ); -int survive_usb_poll( struct SurviveContext * ctx ); -int survive_get_config( char ** config, struct SurviveContext * ctx, int devno, int interface, int send_extra_magic ); //Accept Data from backend. void survive_data_cb( struct SurviveUSBInterface * si ); diff --git a/src/survive_usb.c b/src/survive_usb.c index aec76db..743d805 100644 --- a/src/survive_usb.c +++ b/src/survive_usb.c @@ -10,435 +10,4 @@ //If there are portions of the code too similar to the original, I would like to know so they can be re-written. //All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses. - -#include "survive_internal.h" -#include -#include -#include //sleep if I ever use it. -#include -#include - -const short vidpids[] = { - 0x0bb4, 0x2c87, 0, //The main HTC HMD device - 0x28de, 0x2000, 0, //Valve lighthouse - 0x28de, 0x2101, 0, //Valve Watchman - 0x28de, 0x2101, 1, //Valve Watchman -}; //length MAX_USB_INTERFACES*2 - -const char * devnames[] = { - "HMD", - "Lighthouse", - "Watchman 1", - "Watchman 2", -}; //length MAX_USB_INTERFACES - - -static void handle_transfer(struct libusb_transfer* transfer) -{ - struct SurviveUSBInterface * iface = transfer->user_data; - struct SurviveContext * ctx = iface->ctx; - - if( transfer->status != LIBUSB_TRANSFER_COMPLETED ) - { - SV_ERROR("Transfer problem %d with %s", transfer->status, iface->hname ); - SV_KILL(); - return; - } - - iface->actual_len = transfer->actual_length; - iface->cb( iface ); - - if( libusb_submit_transfer(transfer) ) - { - SV_ERROR( "Error resubmitting transfer for %s", iface->hname ); - SV_KILL(); - } -} - - - -static int AttachInterface( struct SurviveContext * ctx, int which_interface_am_i, libusb_device_handle * devh, int endpoint, usb_callback cb, const char * hname ) -{ - struct SurviveUSBInterface * iface = &ctx->uiface[which_interface_am_i]; - iface->ctx = ctx; - iface->which_interface_am_i = which_interface_am_i; - iface->hname = hname; - struct libusb_transfer * tx = iface->transfer = libusb_alloc_transfer(0); - iface->cb = cb; - //printf( "%p %d %p %p\n", iface, which_interface_am_i, tx, devh ); - - if (!iface->transfer) - { - SV_ERROR( "Error: failed on libusb_alloc_transfer for %s", hname ); - return 4; - } - - libusb_fill_interrupt_transfer( tx, devh, endpoint, iface->buffer, INTBUFFSIZE, handle_transfer, iface, 0); - - int rc = libusb_submit_transfer( tx ); - if( rc ) - { - SV_ERROR( "Error: Could not submit transfer for %s (Code %d)", hname, rc ); - return 6; - } - - return 0; -} - -/* -static void debug_cb( struct SurviveUSBInterface * si ) -{ - int i; - int len = si->actual_len; - printf( "%16s: %d: ", si->hname, len ); - for( i = 0; i < len; i++ ) - { - printf( "%02x ", si->buffer[i] ); - } - printf( "\n" ); -}*/ - -//XXX TODO: Redo this subsystem for setting/updating feature reports. - -static inline int update_feature_report(libusb_device_handle* dev, uint16_t interface, uint8_t * data, int datalen ) { -// int xfer; -// int r = libusb_interrupt_transfer(dev, 0x01, data, datalen, &xfer, 1000); -// printf( "XFER: %d / R: %d\n", xfer, r ); -// return xfer; - return libusb_control_transfer(dev, LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_INTERFACE | LIBUSB_ENDPOINT_OUT, - 0x09, 0x300 | data[0], interface, data, datalen, 1000 ); -} - - -static inline int getupdate_feature_report(libusb_device_handle* dev, uint16_t interface, uint8_t * data, int datalen ) { - - int ret = libusb_control_transfer(dev, LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_INTERFACE | LIBUSB_ENDPOINT_IN, - 0x01, 0x300 | data[0], interface, data, datalen, 1000 ); - if( ret == -9 ) return -9; - if (ret < 0) - return -1; - return ret; -} - - - -static inline int hid_get_feature_report_timeout(libusb_device_handle* device, uint16_t interface, unsigned char *buf, size_t len ) -{ - int ret; - uint8_t i = 0; - for (i = 0; i < 100; i++) - { - ret = getupdate_feature_report(device, interface, buf, len); - if( ret != -9 && ( ret != -1 || errno != EPIPE ) ) return ret; - usleep( 1000 ); - } - - return -1; -} - - -int survive_usb_init( struct SurviveContext * ctx ) -{ - int r = libusb_init( &ctx->usbctx ); - if( r ) - { - SV_ERROR( "libusb fault %d\n", r ); - return r; - } - - int i; - int16_t j; - libusb_device** devs; - int ret = libusb_get_device_list(ctx->usbctx, &devs); - - if( ret < 0 ) - { - SV_ERROR( "Couldn't get list of USB devices %d", ret ); - return ret; - } - - - //Open all interfaces. - for( i = 0; i < MAX_USB_DEVS; i++ ) - { - libusb_device * d; - int vid = vidpids[i*3+0]; - int pid = vidpids[i*3+1]; - int which = vidpids[i*3+2]; - - int did; - for( did = 0; d = devs[did]; did++ ) - { - struct libusb_device_descriptor desc; - - int ret = libusb_get_device_descriptor( d, &desc); - if (ret < 0) { - continue; - } - - if( desc.idVendor == vid && desc.idProduct == pid) - { - if( which == 0 ) break; - which--; - } - } - - if( d == 0 ) - { - SV_ERROR( "Couldn't find device %s (%04x:%04x.%d)", devnames[i], vid, pid, which ); - return -99; - } - - struct libusb_config_descriptor *conf; - ret = libusb_get_config_descriptor(d, 0, &conf); - if( ret ) - continue; - - ret = libusb_open(d, &ctx->udev[i]); - - if( !ctx->udev[i] || ret ) - { - SV_ERROR( "Error: cannot open device \"%s\" with vid/pid %04x:%04x", devnames[i], vid, pid ); - return -5; - } - - libusb_set_auto_detach_kernel_driver( ctx->udev[i], 1 ); - for (j = 0; j < conf->bNumInterfaces; j++ ) - { -#if 0 - if (libusb_kernel_driver_active(ctx->udev[i], j) == 1) { - ret = libusb_detach_kernel_driver(ctx->udev[i], j); - if (ret != LIBUSB_SUCCESS) { - SV_ERROR("Failed to unclaim interface %d for device %s " - "from the kernel.", j, devnames[i] ); - libusb_free_config_descriptor(conf); - libusb_close(ctx->udev[i]); - continue; - } - } -#endif - - if( libusb_claim_interface(ctx->udev[i], j) ) - { - SV_ERROR( "Could not claim interface %d of %s", j, devnames[i] ); - return -9; - } - } - - SV_INFO( "Successfully enumerated %s (%d, %d)", devnames[i], did, conf->bNumInterfaces ); - } - libusb_free_device_list( devs, 1 ); - - if( AttachInterface( ctx, USB_IF_HMD, ctx->udev[USB_DEV_HMD], 0x81, survive_data_cb, "Mainboard" ) ) { return -6; } - if( AttachInterface( ctx, USB_IF_LIGHTHOUSE, ctx->udev[USB_DEV_LIGHTHOUSE], 0x81, survive_data_cb, "Lighthouse" ) ) { return -7; } - if( AttachInterface( ctx, USB_IF_WATCHMAN1, ctx->udev[USB_DEV_WATCHMAN1], 0x81, survive_data_cb, "Watchman 1" ) ) { return -8; } - if( AttachInterface( ctx, USB_IF_WATCHMAN2, ctx->udev[USB_DEV_WATCHMAN2], 0x81, survive_data_cb, "Watchman 2" ) ) { return -9; } - if( AttachInterface( ctx, USB_IF_LIGHTCAP, ctx->udev[USB_DEV_LIGHTHOUSE], 0x82, survive_data_cb, "Lightcap" ) ) { return -10; } - - SV_INFO( "All devices attached." ); - - - //libUSB initialized. Continue. - return 0; -} - -int survive_usb_send_magic(struct SurviveContext * ctx, int turnon ) -{ - int r; - - - if( turnon ) - { - //Magic from vl_magic.h, originally copywritten under LGPL. - // * Copyright (C) 2013 Fredrik Hultin - // * Copyright (C) 2013 Jakob Bornecrantz -#if 0 - static uint8_t vive_magic_power_on[] = { - 0x04, 0x78, 0x29, 0x38, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x01, - 0xa8, 0x0d, 0x76, 0x00, 0x40, 0xfc, 0x01, 0x05, 0xfa, 0xec, 0xd1, 0x6d, 0x00, - 0x00, 0x6c, 0x00, 0x00, 0x00, 0x00, 0x00, 0xa8, 0x0d, 0x76, 0x00, 0x68, 0xfc, - 0x01, 0x05, 0x2c, 0xb0, 0x2e, 0x65, 0x7a, 0x0d, 0x76, 0x00, 0x68, 0x54, 0x72, - 0x00, 0x18, 0x54, 0x72, 0x00, 0x00, 0x6a, 0x72, 0x00, 0x00, 0x00, 0x00, - }; -#else - //From actual steam. - static uint8_t vive_magic_power_on[64] = { 0x04, 0x78, 0x29, 0x38, - 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7a, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; -#endif - r = update_feature_report( ctx->udev[USB_DEV_HMD], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) ); - if( r != sizeof( vive_magic_power_on ) ) return 5; - - static uint8_t vive_magic_enable_lighthouse[64] = { 0x04 }; //[64] wat? Why did that fix it? - r = update_feature_report( ctx->udev[USB_DEV_LIGHTHOUSE], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) ); - if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5; - -#if 0 - for( i = 0; i < 256; i++ ) - { - static uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0xff, 0, 0, 0, 0, 0, 0, 0 }; - r = update_feature_report( ctx->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) ); - SV_INFO( "UCR: %d", r ); - if( r != sizeof( vive_controller_haptic_pulse ) ) return 5; - usleep( 1000 ); - } -#endif - SV_INFO( "Powered unit on." ); - } - else - { - - static uint8_t vive_magic_power_off1[] = { - 0x04, 0x78, 0x29, 0x38, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, - 0x30, 0x05, 0x77, 0x00, 0x30, 0x05, 0x77, 0x00, 0x6c, 0x4d, 0x37, 0x65, 0x40, - 0xf9, 0x33, 0x00, 0x04, 0xf8, 0xa3, 0x04, 0x04, 0x00, 0x00, 0x00, 0x70, 0xb0, - 0x72, 0x00, 0xf4, 0xf7, 0xa3, 0x04, 0x7c, 0xf8, 0x33, 0x00, 0x0c, 0xf8, 0xa3, - 0x04, 0x0a, 0x6e, 0x29, 0x65, 0x24, 0xf9, 0x33, 0x00, 0x00, 0x00, 0x00, - }; - - static uint8_t vive_magic_power_off2[] = { - 0x04, 0x78, 0x29, 0x38, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, - 0x30, 0x05, 0x77, 0x00, 0xe4, 0xf7, 0x33, 0x00, 0xe4, 0xf7, 0x33, 0x00, 0x60, - 0x6e, 0x72, 0x00, 0xb4, 0xf7, 0x33, 0x00, 0x04, 0x00, 0x00, 0x00, 0x70, 0xb0, - 0x72, 0x00, 0x90, 0xf7, 0x33, 0x00, 0x7c, 0xf8, 0x33, 0x00, 0xd0, 0xf7, 0x33, - 0x00, 0x3c, 0x68, 0x29, 0x65, 0x24, 0xf9, 0x33, 0x00, 0x00, 0x00, 0x00, - }; - -// r = update_feature_report( ctx->udev[USB_DEV_HMD], 0, vive_magic_power_off1, sizeof( vive_magic_power_off1 ) ); -// SV_INFO( "UCR: %d", r ); -// if( r != sizeof( vive_magic_power_off1 ) ) return 5; - - r = update_feature_report( ctx->udev[USB_DEV_HMD], 0, vive_magic_power_off2, sizeof( vive_magic_power_off2 ) ); - SV_INFO( "UCR: %d", r ); - if( r != sizeof( vive_magic_power_off2 ) ) return 5; - - } - -} - -void survive_usb_close( struct SurviveContext * t ) -{ - int i; - for( i = 0; i < MAX_USB_DEVS; i++ ) - { - libusb_close( t->udev[i] ); - } - libusb_exit(t->usbctx); -} - -int survive_usb_poll( struct SurviveContext * ctx ) -{ - int r = libusb_handle_events( ctx->usbctx ); - if( r ) - { - SV_ERROR( "Libusb poll failed." ); - } - return r; -} - - -int survive_get_config( char ** config, struct SurviveContext * ctx, int devno, int iface, int send_extra_magic ) -{ - int i, ret, count = 0, size = 0; - uint8_t cfgbuff[64]; - uint8_t compressed_data[8192]; - uint8_t uncompressed_data[65536]; - struct libusb_device_handle * dev = ctx->udev[devno]; - - if( send_extra_magic ) - { - uint8_t cfgbuffwide[65]; - - memset( cfgbuffwide, 0, sizeof( cfgbuff ) ); - cfgbuffwide[0] = 0x01; - ret = hid_get_feature_report_timeout( dev, iface, cfgbuffwide, sizeof( cfgbuffwide ) ); - usleep(1000); - - int k; - - //Switch mode to pull config? - for( k = 0; k < 10; k++ ) - { - uint8_t cfgbuff_send[64] = { - 0xff, 0x83, 0x00, 0xb6, 0x5b, 0xb0, 0x78, 0x69, - 0x0f, 0xf8, 0x78, 0x69, 0x0f, 0xa0, 0xf3, 0x18, - 0x00, 0xe8, 0xf2, 0x18, 0x00, 0x27, 0x44, 0x5a, - 0x0f, 0xf8, 0x78, 0x69, 0x0f, 0xf0, 0x77, 0x69, - 0x0f, 0xf0, 0x77, 0x69, 0x0f, 0x50, 0xca, 0x45, - 0x77, 0xa0, 0xf3, 0x18, 0x00, 0xf8, 0x78, 0x69, - 0x0f, 0x00, 0x00, 0xa0, 0x0f, 0xa0, 0x9b, 0x0a, - 0x01, 0x00, 0x00, 0x35, 0x00, 0x34, 0x02, 0x00 - }; - - int rk = libusb_control_transfer(dev, LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_INTERFACE | LIBUSB_ENDPOINT_OUT, - 0x09, 0x300 | cfgbuff_send[0], iface, cfgbuff_send, 64, 1000 ); - usleep(1000); - } - - cfgbuffwide[0] = 0xff; - ret = hid_get_feature_report_timeout( dev, iface, cfgbuffwide, sizeof( cfgbuffwide ) ); - usleep(1000); - } - - memset( cfgbuff, 0, sizeof( cfgbuff ) ); - cfgbuff[0] = 0x10; - if( ( ret = hid_get_feature_report_timeout( dev, iface, cfgbuff, sizeof( cfgbuff ) ) ) < 0 ) - { - SV_INFO( "Could not get survive config data for device %d:%d", devno, iface ); - return -1; - } - - - cfgbuff[1] = 0xaa; - cfgbuff[0] = 0x11; - do - { - if( (ret = hid_get_feature_report_timeout(dev, iface, cfgbuff, sizeof( cfgbuff ) ) ) < 0 ) - { - SV_INFO( "Could not read config data (after first packet) on device %d:%d (count: %d)\n", devno, iface, count ); - return -2; - } - - size = cfgbuff[1]; - - if( !size ) break; - - if( size > 62 ) - { - SV_INFO( "Too much data (%d) on packet from config for device %d:%d (count: %d)", size, devno, iface, count ); - return -3; - } - - if( count + size >= sizeof( compressed_data ) ) - { - SV_INFO( "Configuration length too long %d:%d (count: %d)", devno, iface, count ); - return -4; - } - - memcpy( &compressed_data[count], cfgbuff + 2, size ); - count += size; - } while( 1 ); - - if( count == 0 ) - { - SV_INFO( "Empty configuration for %d:%d", devno, iface ); - return -5; - } - - SV_INFO( "Got config data length %d", count ); - - int len = survive_simple_inflate( ctx, compressed_data, count, uncompressed_data, sizeof(uncompressed_data)-1 ); - if( len <= 0 ) - { - SV_INFO( "Error: data for config descriptor %d:%d is bad.", devno, iface ); - return -5; - } - - *config = malloc( len + 1 ); - memcpy( *config, uncompressed_data, len ); - return len; -} - - +//No code? diff --git a/src/survive_vive.c b/src/survive_vive.c new file mode 100644 index 0000000..a402153 --- /dev/null +++ b/src/survive_vive.c @@ -0,0 +1,1075 @@ +//Unofficial driver for the official Valve/HTC Vive hardware. +// +//Based off of https://github.com/collabora/OSVR-Vive-Libre +// Originally Copyright 2016 Philipp Zabel +// Originally Copyright 2016 Lubosz Sarnecki +// Originally Copyright (C) 2013 Fredrik Hultin +// Originally Copyright (C) 2013 Jakob Bornecrantz +// +//But, re-written as best as I can to get it put under an open souce license instead of a forced-source license. +//If there are portions of the code too similar to the original, I would like to know so they can be re-written. +//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses. + +#include "survive_internal.h" +#include "survive_driverman.h" +#include + +#include "survive_internal.h" +#include +#include +#include //sleep if I ever use it. +#include +#include + +struct SurviveViveData; + +//USB Subsystem +void survive_usb_close( struct SurviveContext * t ); +int survive_usb_init( struct SurviveViveData * sv, struct SurviveObject * hmd, struct SurviveObject *wm0, struct SurviveObject * wm1 ); +int survive_usb_poll( struct SurviveContext * ctx ); +int survive_get_config( char ** config, struct SurviveViveData * ctx, int devno, int interface, int send_extra_magic ); + + +const short vidpids[] = { + 0x0bb4, 0x2c87, 0, //The main HTC HMD device + 0x28de, 0x2000, 0, //Valve lighthouse + 0x28de, 0x2101, 0, //Valve Watchman + 0x28de, 0x2101, 1, //Valve Watchman +}; //length MAX_USB_INTERFACES*2 + +const char * devnames[] = { + "HMD", + "Lighthouse", + "Watchman 1", + "Watchman 2", +}; //length MAX_USB_INTERFACES + + +#define USB_DEV_HMD 0 +#define USB_DEV_LIGHTHOUSE 1 +#define USB_DEV_WATCHMAN1 2 +#define USB_DEV_WATCHMAN2 3 +#define MAX_USB_DEVS 4 + + +#define USB_IF_HMD 0 +#define USB_IF_LIGHTHOUSE 1 +#define USB_IF_WATCHMAN1 2 +#define USB_IF_WATCHMAN2 3 +#define USB_IF_LIGHTCAP 4 +#define MAX_INTERFACES 5 + +typedef void (*usb_callback)( struct SurviveUSBInterface * ti ); + +struct SurviveUSBInterface +{ + struct SurviveViveData * sv; + struct SurviveContext * ctx; + + struct libusb_transfer * transfer; + struct SurviveObject * assoc_obj; + int actual_len; + uint8_t buffer[INTBUFFSIZE]; + usb_callback cb; + int which_interface_am_i; //for indexing into uiface + const char * hname; //human-readable names +}; + +struct SurviveViveData +{ + struct SurviveContext * ctx; + + //XXX TODO: UN-STATICIFY THIS. + struct SurviveUSBInterface uiface[MAX_INTERFACES]; + //USB Subsystem + struct libusb_context* usbctx; + struct libusb_device_handle * udev[MAX_USB_DEVS]; + +}; + + + + +static void handle_transfer(struct libusb_transfer* transfer) +{ + struct SurviveUSBInterface * iface = transfer->user_data; + struct SurviveContext * ctx = iface->ctx; + + if( transfer->status != LIBUSB_TRANSFER_COMPLETED ) + { + SV_ERROR("Transfer problem %d with %s", transfer->status, iface->hname ); + SV_KILL(); + return; + } + + iface->actual_len = transfer->actual_length; + iface->cb( iface ); + + if( libusb_submit_transfer(transfer) ) + { + SV_ERROR( "Error resubmitting transfer for %s", iface->hname ); + SV_KILL(); + } +} + + + +static int AttachInterface( struct SurviveViveData * sv, struct SurviveObject * assocobj, int which_interface_am_i, libusb_device_handle * devh, int endpoint, usb_callback cb, const char * hname ) +{ + struct SurviveContext * ctx = sv->ctx; + struct SurviveUSBInterface * iface = &sv->uiface[which_interface_am_i]; + iface->ctx = ctx; + iface->sv = sv; + iface->which_interface_am_i = which_interface_am_i; + iface->assoc_obj = assocobj; + iface->hname = hname; + struct libusb_transfer * tx = iface->transfer = libusb_alloc_transfer(0); + iface->cb = cb; + //printf( "%p %d %p %p\n", iface, which_interface_am_i, tx, devh ); + + if (!iface->transfer) + { + SV_ERROR( "Error: failed on libusb_alloc_transfer for %s", hname ); + return 4; + } + + libusb_fill_interrupt_transfer( tx, devh, endpoint, iface->buffer, INTBUFFSIZE, handle_transfer, iface, 0); + + int rc = libusb_submit_transfer( tx ); + if( rc ) + { + SV_ERROR( "Error: Could not submit transfer for %s (Code %d)", hname, rc ); + return 6; + } + + return 0; +} + +/* +static void debug_cb( struct SurviveUSBInterface * si ) +{ + int i; + int len = si->actual_len; + printf( "%16s: %d: ", si->hname, len ); + for( i = 0; i < len; i++ ) + { + printf( "%02x ", si->buffer[i] ); + } + printf( "\n" ); +}*/ + +//XXX TODO: Redo this subsystem for setting/updating feature reports. + +static inline int update_feature_report(libusb_device_handle* dev, uint16_t interface, uint8_t * data, int datalen ) { +// int xfer; +// int r = libusb_interrupt_transfer(dev, 0x01, data, datalen, &xfer, 1000); +// printf( "XFER: %d / R: %d\n", xfer, r ); +// return xfer; + return libusb_control_transfer(dev, LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_INTERFACE | LIBUSB_ENDPOINT_OUT, + 0x09, 0x300 | data[0], interface, data, datalen, 1000 ); +} + + +static inline int getupdate_feature_report(libusb_device_handle* dev, uint16_t interface, uint8_t * data, int datalen ) { + + int ret = libusb_control_transfer(dev, LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_INTERFACE | LIBUSB_ENDPOINT_IN, + 0x01, 0x300 | data[0], interface, data, datalen, 1000 ); + if( ret == -9 ) return -9; + if (ret < 0) + return -1; + return ret; +} + + + +static inline int hid_get_feature_report_timeout(libusb_device_handle* device, uint16_t interface, unsigned char *buf, size_t len ) +{ + int ret; + uint8_t i = 0; + for (i = 0; i < 100; i++) + { + ret = getupdate_feature_report(device, interface, buf, len); + if( ret != -9 && ( ret != -1 || errno != EPIPE ) ) return ret; + usleep( 1000 ); + } + + return -1; +} + + +int survive_usb_init( struct SurviveViveData * sv, struct SurviveObject * hmd, struct SurviveObject *wm0, struct SurviveObject * wm1 ) +{ + struct SurviveContext * ctx = sv->ctx; + int r = libusb_init( &sv->usbctx ); + if( r ) + { + SV_ERROR( "libusb fault %d\n", r ); + return r; + } + + int i; + int16_t j; + libusb_device** devs; + int ret = libusb_get_device_list(sv->usbctx, &devs); + + if( ret < 0 ) + { + SV_ERROR( "Couldn't get list of USB devices %d", ret ); + return ret; + } + + + //Open all interfaces. + for( i = 0; i < MAX_USB_DEVS; i++ ) + { + libusb_device * d; + int vid = vidpids[i*3+0]; + int pid = vidpids[i*3+1]; + int which = vidpids[i*3+2]; + + int did; + for( did = 0; d = devs[did]; did++ ) + { + struct libusb_device_descriptor desc; + + int ret = libusb_get_device_descriptor( d, &desc); + if (ret < 0) { + continue; + } + + if( desc.idVendor == vid && desc.idProduct == pid) + { + if( which == 0 ) break; + which--; + } + } + + if( d == 0 ) + { + SV_ERROR( "Couldn't find device %s (%04x:%04x.%d)", devnames[i], vid, pid, which ); + return -99; + } + + struct libusb_config_descriptor *conf; + ret = libusb_get_config_descriptor(d, 0, &conf); + if( ret ) + continue; + + ret = libusb_open(d, &sv->udev[i]); + + if( !sv->udev[i] || ret ) + { + SV_ERROR( "Error: cannot open device \"%s\" with vid/pid %04x:%04x", devnames[i], vid, pid ); + return -5; + } + + libusb_set_auto_detach_kernel_driver( sv->udev[i], 1 ); + for (j = 0; j < conf->bNumInterfaces; j++ ) + { +#if 0 + if (libusb_kernel_driver_active(sv->udev[i], j) == 1) { + ret = libusb_detach_kernel_driver(sv->udev[i], j); + if (ret != LIBUSB_SUCCESS) { + SV_ERROR("Failed to unclaim interface %d for device %s " + "from the kernel.", j, devnames[i] ); + libusb_free_config_descriptor(conf); + libusb_close(sv->udev[i]); + continue; + } + } +#endif + + if( libusb_claim_interface(sv->udev[i], j) ) + { + SV_ERROR( "Could not claim interface %d of %s", j, devnames[i] ); + return -9; + } + } + + SV_INFO( "Successfully enumerated %s (%d, %d)", devnames[i], did, conf->bNumInterfaces ); + } + libusb_free_device_list( devs, 1 ); + + if( AttachInterface( sv, hmd, USB_IF_HMD, sv->udev[USB_DEV_HMD], 0x81, survive_data_cb, "Mainboard" ) ) { return -6; } + if( AttachInterface( sv, hmd, USB_IF_LIGHTHOUSE, sv->udev[USB_DEV_LIGHTHOUSE], 0x81, survive_data_cb, "Lighthouse" ) ) { return -7; } + if( AttachInterface( sv, wm0, USB_IF_WATCHMAN1, sv->udev[USB_DEV_WATCHMAN1], 0x81, survive_data_cb, "Watchman 1" ) ) { return -8; } + if( AttachInterface( sv, wm1, USB_IF_WATCHMAN2, sv->udev[USB_DEV_WATCHMAN2], 0x81, survive_data_cb, "Watchman 2" ) ) { return -9; } + if( AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_LIGHTHOUSE], 0x82, survive_data_cb, "Lightcap" ) ) { return -10; } + + SV_INFO( "All devices attached." ); + + + //libUSB initialized. Continue. + return 0; +} + +int survive_vive_send_magic(struct SurviveContext * ctx, void * drv, int magic_code, void * data, int datalen ) +{ + int r; + struct SurviveViveData * sv = drv; + printf( "*CALLING %p %p\n", ctx, sv ); + + //XXX TODO: Handle haptics, etc. + int turnon = magic_code; + + + if( turnon ) + { + //Magic from vl_magic.h, originally copywritten under LGPL. + // * Copyright (C) 2013 Fredrik Hultin + // * Copyright (C) 2013 Jakob Bornecrantz +#if 0 + static uint8_t vive_magic_power_on[] = { + 0x04, 0x78, 0x29, 0x38, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x01, + 0xa8, 0x0d, 0x76, 0x00, 0x40, 0xfc, 0x01, 0x05, 0xfa, 0xec, 0xd1, 0x6d, 0x00, + 0x00, 0x6c, 0x00, 0x00, 0x00, 0x00, 0x00, 0xa8, 0x0d, 0x76, 0x00, 0x68, 0xfc, + 0x01, 0x05, 0x2c, 0xb0, 0x2e, 0x65, 0x7a, 0x0d, 0x76, 0x00, 0x68, 0x54, 0x72, + 0x00, 0x18, 0x54, 0x72, 0x00, 0x00, 0x6a, 0x72, 0x00, 0x00, 0x00, 0x00, + }; +#else + //From actual steam. + static uint8_t vive_magic_power_on[64] = { 0x04, 0x78, 0x29, 0x38, + 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7a, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; +#endif + r = update_feature_report( sv->udev[USB_DEV_HMD], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) ); + if( r != sizeof( vive_magic_power_on ) ) return 5; + + static uint8_t vive_magic_enable_lighthouse[64] = { 0x04 }; //[64] wat? Why did that fix it? + r = update_feature_report( sv->udev[USB_DEV_LIGHTHOUSE], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) ); + if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5; + +#if 0 + for( i = 0; i < 256; i++ ) + { + static uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0xff, 0, 0, 0, 0, 0, 0, 0 }; + r = update_feature_report( sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) ); + SV_INFO( "UCR: %d", r ); + if( r != sizeof( vive_controller_haptic_pulse ) ) return 5; + usleep( 1000 ); + } +#endif + SV_INFO( "Powered unit on." ); + } + else + { + + static uint8_t vive_magic_power_off1[] = { + 0x04, 0x78, 0x29, 0x38, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, + 0x30, 0x05, 0x77, 0x00, 0x30, 0x05, 0x77, 0x00, 0x6c, 0x4d, 0x37, 0x65, 0x40, + 0xf9, 0x33, 0x00, 0x04, 0xf8, 0xa3, 0x04, 0x04, 0x00, 0x00, 0x00, 0x70, 0xb0, + 0x72, 0x00, 0xf4, 0xf7, 0xa3, 0x04, 0x7c, 0xf8, 0x33, 0x00, 0x0c, 0xf8, 0xa3, + 0x04, 0x0a, 0x6e, 0x29, 0x65, 0x24, 0xf9, 0x33, 0x00, 0x00, 0x00, 0x00, + }; + + static uint8_t vive_magic_power_off2[] = { + 0x04, 0x78, 0x29, 0x38, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, + 0x30, 0x05, 0x77, 0x00, 0xe4, 0xf7, 0x33, 0x00, 0xe4, 0xf7, 0x33, 0x00, 0x60, + 0x6e, 0x72, 0x00, 0xb4, 0xf7, 0x33, 0x00, 0x04, 0x00, 0x00, 0x00, 0x70, 0xb0, + 0x72, 0x00, 0x90, 0xf7, 0x33, 0x00, 0x7c, 0xf8, 0x33, 0x00, 0xd0, 0xf7, 0x33, + 0x00, 0x3c, 0x68, 0x29, 0x65, 0x24, 0xf9, 0x33, 0x00, 0x00, 0x00, 0x00, + }; + +// r = update_feature_report( sv->udev[USB_DEV_HMD], 0, vive_magic_power_off1, sizeof( vive_magic_power_off1 ) ); +// SV_INFO( "UCR: %d", r ); +// if( r != sizeof( vive_magic_power_off1 ) ) return 5; + + r = update_feature_report( sv->udev[USB_DEV_HMD], 0, vive_magic_power_off2, sizeof( vive_magic_power_off2 ) ); + SV_INFO( "UCR: %d", r ); + if( r != sizeof( vive_magic_power_off2 ) ) return 5; + + } + +} + +void survive_vive_usb_close( struct SurviveViveData * sv ) +{ + int i; + for( i = 0; i < MAX_USB_DEVS; i++ ) + { + libusb_close( sv->udev[i] ); + } + libusb_exit(sv->usbctx); +} + +int survive_vive_usb_poll( struct SurviveContext * ctx, void * v ) +{ + struct SurviveViveData * sv = v; + int r = libusb_handle_events( sv->usbctx ); + if( r ) + { + struct SurviveContext * ctx = sv->ctx; + SV_ERROR( "Libusb poll failed." ); + } + return r; +} + + +int survive_get_config( char ** config, struct SurviveViveData * sv, int devno, int iface, int send_extra_magic ) +{ + struct SurviveContext * ctx = sv->ctx; + int i, ret, count = 0, size = 0; + uint8_t cfgbuff[64]; + uint8_t compressed_data[8192]; + uint8_t uncompressed_data[65536]; + struct libusb_device_handle * dev = sv->udev[devno]; + + if( send_extra_magic ) + { + uint8_t cfgbuffwide[65]; + + memset( cfgbuffwide, 0, sizeof( cfgbuff ) ); + cfgbuffwide[0] = 0x01; + ret = hid_get_feature_report_timeout( dev, iface, cfgbuffwide, sizeof( cfgbuffwide ) ); + usleep(1000); + + int k; + + //Switch mode to pull config? + for( k = 0; k < 10; k++ ) + { + uint8_t cfgbuff_send[64] = { + 0xff, 0x83, 0x00, 0xb6, 0x5b, 0xb0, 0x78, 0x69, + 0x0f, 0xf8, 0x78, 0x69, 0x0f, 0xa0, 0xf3, 0x18, + 0x00, 0xe8, 0xf2, 0x18, 0x00, 0x27, 0x44, 0x5a, + 0x0f, 0xf8, 0x78, 0x69, 0x0f, 0xf0, 0x77, 0x69, + 0x0f, 0xf0, 0x77, 0x69, 0x0f, 0x50, 0xca, 0x45, + 0x77, 0xa0, 0xf3, 0x18, 0x00, 0xf8, 0x78, 0x69, + 0x0f, 0x00, 0x00, 0xa0, 0x0f, 0xa0, 0x9b, 0x0a, + 0x01, 0x00, 0x00, 0x35, 0x00, 0x34, 0x02, 0x00 + }; + + int rk = libusb_control_transfer(dev, LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_INTERFACE | LIBUSB_ENDPOINT_OUT, + 0x09, 0x300 | cfgbuff_send[0], iface, cfgbuff_send, 64, 1000 ); + usleep(1000); + } + + cfgbuffwide[0] = 0xff; + ret = hid_get_feature_report_timeout( dev, iface, cfgbuffwide, sizeof( cfgbuffwide ) ); + usleep(1000); + } + + memset( cfgbuff, 0, sizeof( cfgbuff ) ); + cfgbuff[0] = 0x10; + if( ( ret = hid_get_feature_report_timeout( dev, iface, cfgbuff, sizeof( cfgbuff ) ) ) < 0 ) + { + SV_INFO( "Could not get survive config data for device %d:%d", devno, iface ); + return -1; + } + + + cfgbuff[1] = 0xaa; + cfgbuff[0] = 0x11; + do + { + if( (ret = hid_get_feature_report_timeout(dev, iface, cfgbuff, sizeof( cfgbuff ) ) ) < 0 ) + { + SV_INFO( "Could not read config data (after first packet) on device %d:%d (count: %d)\n", devno, iface, count ); + return -2; + } + + size = cfgbuff[1]; + + if( !size ) break; + + if( size > 62 ) + { + SV_INFO( "Too much data (%d) on packet from config for device %d:%d (count: %d)", size, devno, iface, count ); + return -3; + } + + if( count + size >= sizeof( compressed_data ) ) + { + SV_INFO( "Configuration length too long %d:%d (count: %d)", devno, iface, count ); + return -4; + } + + memcpy( &compressed_data[count], cfgbuff + 2, size ); + count += size; + } while( 1 ); + + if( count == 0 ) + { + SV_INFO( "Empty configuration for %d:%d", devno, iface ); + return -5; + } + + SV_INFO( "Got config data length %d", count ); + + int len = survive_simple_inflate( ctx, compressed_data, count, uncompressed_data, sizeof(uncompressed_data)-1 ); + if( len <= 0 ) + { + SV_INFO( "Error: data for config descriptor %d:%d is bad.", devno, iface ); + return -5; + } + + *config = malloc( len + 1 ); + memcpy( *config, uncompressed_data, len ); + return len; +} + +/////////////////////////////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////// + +#define POP1 (*(readdata++)) +#define POP2 (*(((uint16_t*)((readdata+=2)-2)))) +#define POP4 (*(((uint32_t*)((readdata+=4)-4)))) + +static void handle_watchman( struct SurviveObject * w, uint8_t * readdata ) +{ + int i; + + uint8_t startread[29]; + memcpy( startread, readdata, 29 ); + +#if 0 + printf( "DAT: " ); + for( i = 0; i < 29; i++ ) + { + printf( "%02x ", readdata[i] ); + } + printf("\n"); +#endif + + uint8_t time1 = POP1; + uint8_t qty = POP1; + uint8_t time2 = POP1; + uint8_t type = POP1; + qty-=2; + int propset = 0; + int doimu = 0; + + + if( (type & 0xf0) == 0xf0 ) + { + propset |= 4; + //printf( "%02x %02x %02x %02x\n", qty, type, time1, time2 ); + type &= ~0x10; + + if( type & 0x01 ) + { + qty-=1; + w->buttonmask = POP1; + type &= ~0x01; + } + if( type & 0x04 ) + { + qty-=1; + w->axis1 = ( POP1 ) * 128; + type &= ~0x04; + } + if( type & 0x02 ) + { + qty-=4; + w->axis2 = POP2; + w->axis3 = POP2; + type &= ~0x02; + } + + //XXX TODO: Is this correct? It looks SO WACKY + type &= 0x7f; + if( type == 0x68 ) doimu = 1; + type &= 0x0f; + if( type == 0x00 && qty ) { type = POP1; qty--; } + } + + if( type == 0xe1 ) + { + propset |= 1; + w->charging = readdata[0]>>7; + w->charge = POP1&0x7f; qty--; + w->ison = 1; + if( qty ) + { + qty--; + type = POP1; //IMU usually follows. + } + } + + if( ( ( type & 0xe8 ) == 0xe8 ) || doimu ) //Hmm, this looks kind of yucky... we can get e8's that are accelgyro's but, cleared by first propset. + { + propset |= 2; + w->ctx->imuproc( w, (int16_t *)&readdata[1], (time1<<24)|(time2<<16)|readdata[0], 0 ); + int16_t * k = (int16_t *)readdata+1; + //printf( "Match8 %d %d %d %d %d %3d %3d\n", qty, k[0], k[1], k[2], k[3], k[4], k[5] ); + readdata += 13; qty -= 13; + type &= ~0xe8; + if( qty ) + { + qty--; + type = POP1; + } + } + + + if( qty ) + { + int j; + qty++; + readdata--; + *readdata = type; //Put 'type' back on stack. + uint8_t * mptr = readdata + qty-3-1; //-3 for timecode, -1 to + +//#define DEBUG_WATCHMAN +#ifdef DEBUG_WATCHMAN + printf( "_%s ", w->codename); + for( i = 0; i < qty; i++ ) + { + printf( "%02x ", readdata[i] ); + } + printf("\n"); +#endif + + + uint32_t mytime = (mptr[3] << 16)|(mptr[2] << 8)|(mptr[1] << 0); + + uint32_t times[20]; + const int nrtime = sizeof(times)/sizeof(uint32_t); + int timecount = 0; + int leds; + int parameters; + int fault = 0; + + ///Handle uint32_tifying (making sure we keep it incrementing) + uint32_t llt = w->last_lighttime; + uint32_t imumsb = time1<<24; + mytime |= imumsb; + + //Compare mytime to llt + + int diff = mytime - llt; + if( diff < -0x1000000 ) + mytime += 0x1000000; + else if( diff > 0x100000 ) + mytime -= 0x1000000; + + w->last_lighttime = mytime; + + times[timecount++] = mytime; +#ifdef DEBUG_WATCHMAN + printf( "_%s Packet Start Time: %d\n", w->codename, mytime ); +#endif + + //First, pull off the times, starting with the current time, then all the delta times going backwards. + { + while( mptr - readdata > (timecount>>1) ) + { + uint32_t arcane_value = 0; + //ArcanePop (Pop off values from the back, forward, checking if the MSB is set) + do + { + uint8_t ap = *(mptr--); + arcane_value |= (ap&0x7f); + if( ap & 0x80 ) break; + arcane_value <<= 7; + } while(1); + times[timecount++] = (mytime -= arcane_value); +#ifdef DEBUG_WATCHMAN + printf( "_%s Time: %d newtime: %d\n", w->codename, arcane_value, mytime ); +#endif + } + + leds = timecount>>1; + //Check that the # of sensors at the beginning match the # of parameters we would expect. + if( timecount & 1 ) { fault = 1; goto end; } //Inordinal LED count + if( leds != mptr - readdata + 1 ) { fault = 2; goto end; } //LED Count does not line up with parameters + } + + + struct LightcapElement les[10]; + int lese = 0; //les's end + + //Second, go through all LEDs and extract the lightevent from them. + { + uint8_t marked[nrtime]; + memset( marked, 0, sizeof( marked ) ); + int i, parpl = 0; + timecount--; + int timepl = 0; + + //This works, but usually returns the values in reverse end-time order. + for( i = 0; i < leds; i++ ) + { + int led = readdata[i]; + int adv = led & 0x07; + led >>= 3; + + while( marked[timepl] ) timepl++; + if( timepl > timecount ) { fault = 3; goto end; } //Ran off max of list. + uint32_t endtime = times[timepl++]; + int end = timepl + adv; + if( end > timecount ) { fault = 4; goto end; } //end referencing off list + if( marked[end] > 0 ) { fault = 5; goto end; } //Already marked trying to be used. + uint32_t starttime = times[end]; + marked[end] = 1; + + //Insert all lighting things into a sorted list. This list will be + //reverse sorted, but that is to minimize operations. To read it + //in sorted order simply read it back backwards. + //Use insertion sort, since we should most of the time, be in order. + struct LightcapElement * le = &les[lese++]; + le->sensor_id = led; + le->type = 0xfe; + + if( (uint32_t)(endtime - starttime) > 65535 ) { fault = 6; goto end; } //Length of pulse dumb. + le->length = endtime - starttime; + le->timestamp = starttime; + +#ifdef DEBUG_WATCHMAN + printf( "_%s Event: %d %d %d-%d\n", w->codename, led, le->length, endtime, starttime ); +#endif + int swap = lese-2; + while( swap >= 0 && les[swap].timestamp < les[swap+1].timestamp ) + { + struct LightcapElement l; + memcpy( &l, &les[swap], sizeof( l ) ); + memcpy( &les[swap], &les[swap+1], sizeof( l ) ); + memcpy( &les[swap+1], &l, sizeof( l ) ); + swap--; + } + } + } + + int i; + for( i = lese-1; i >= 0; i-- ) + { + //printf( "%d: %d [%d]\n", les[i].sensor_id, les[i].length, les[i].timestamp ); + handle_lightcap( w, &les[i] ); + } + + return; + end: + { + struct SurviveContext * ctx = w->ctx; + SV_INFO( "Light decoding fault: %d\n", fault ); + } + } +} + + +void survive_data_cb( struct SurviveUSBInterface * si ) +{ + int size = si->actual_len; + struct SurviveContext * ctx = si->ctx; +#if 0 + int i; + printf( "%16s: %d: ", si->hname, len ); + for( i = 0; i < size; i++ ) + { + printf( "%02x ", si->buffer[i] ); + } + printf( "\n" ); + return; +#endif + + int iface = si->which_interface_am_i; + struct SurviveObject * obj = si->assoc_obj; + uint8_t * readdata = si->buffer; + + int id = POP1; +// printf( "%16s Size: %2d ID: %d / %d\n", si->hname, size, id, iface ); + + + switch( si->which_interface_am_i ) + { + case USB_IF_HMD: + { + struct SurviveObject * headset = obj; + readdata+=2; + headset->buttonmask = POP1; //Lens + headset->axis2 = POP2; //Lens Separation + readdata+=2; + headset->buttonmask |= POP1; //Button + readdata+=3; + readdata++; //Proxchange, No change = 0, Decrease = 1, Increase = 2 + readdata++; + headset->axis3 = POP2; //Proximity << how close to face are you? Less than 80 = not on face. + headset->axis1 = POP2; //IPD << what is this? + headset->ison = 1; + break; + } + case USB_IF_LIGHTHOUSE: + { + int i; + //printf( "%d -> ", size ); + for( i = 0; i < 3; i++ ) + { + int16_t * acceldata = (int16_t*)readdata; + readdata += 12; + uint32_t timecode = POP4; + uint8_t code = POP1; + //printf( "%d ", code ); + int8_t cd = code - obj->oldcode; + + if( cd > 0 ) + { + obj->oldcode = code; + ctx->imuproc( obj, acceldata, timecode, code ); + } + } + + //DONE OK. + break; + } + case USB_IF_WATCHMAN1: + case USB_IF_WATCHMAN2: + { + struct SurviveObject * w = obj; + if( id == 35 ) + { + handle_watchman( w, readdata); + } + else if( id == 36 ) + { + handle_watchman( w, readdata); + handle_watchman( w, readdata+29 ); + } + else if( id == 38 ) + { + w->ison = 0; + } + else + { + SV_INFO( "Unknown watchman code %d\n", id ); + } + break; + } + case USB_IF_LIGHTCAP: + { + //Done! + int i; + for( i = 0; i < 7; i++ ) + { + handle_lightcap( obj, (struct LightcapElement*)&readdata[i*8] ); + } + break; + } + } +} + + + + + + + + + + +/////////////////////////////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////// + + + +static int jsoneq(const char *json, jsmntok_t *tok, const char *s) { + if (tok->type == JSMN_STRING && (int) strlen(s) == tok->end - tok->start && + strncmp(json + tok->start, s, tok->end - tok->start) == 0) { + return 0; + } + return -1; +} + + +static int ParsePoints( struct SurviveContext * ctx, struct SurviveObject * so, char * ct0conf, FLT ** floats_out, jsmntok_t * t, int i ) +{ + int k; + int pts = t[i+1].size; + jsmntok_t * tk; + + so->nr_locations = 0; + *floats_out = malloc( sizeof( **floats_out ) * 32 * 3 ); + + for( k = 0; k < pts; k++ ) + { + tk = &t[i+2+k*4]; + + FLT vals[3]; + int m; + for( m = 0; m < 3; m++ ) + { + char ctt[128]; + + tk++; + int elemlen = tk->end - tk->start; + + if( tk->type != 4 || elemlen > sizeof( ctt )-1 ) + { + SV_ERROR( "Parse error in JSON\n" ); + return 1; + } + + memcpy( ctt, ct0conf + tk->start, elemlen ); + ctt[elemlen] = 0; + FLT f = atof( ctt ); + int id = so->nr_locations*3+m; + (*floats_out)[id] = f; + } + so->nr_locations++; + } + return 0; +} + +static int LoadConfig( struct SurviveViveData * sv, struct SurviveObject * so, int devno, int iface, int extra_magic ) +{ + struct SurviveContext * ctx = sv->ctx; + char * ct0conf = 0; + int len = survive_get_config( &ct0conf, sv, devno, iface, extra_magic ); + +#if 0 + char fname[100]; + sprintf( fname, "%s_config.json", so->codename ); + FILE * f = fopen( fname, "w" ); + fwrite( ct0conf, strlen(ct0conf), 1, f ); + fclose( f ); +#endif + + if( len > 0 ) + { + + //From JSMN example. + jsmn_parser p; + jsmntok_t t[4096]; + jsmn_init(&p); + int i; + int r = jsmn_parse(&p, ct0conf, len, t, sizeof(t)/sizeof(t[0])); + if (r < 0) { + SV_INFO("Failed to parse JSON in HMD configuration: %d\n", r); + return -1; + } + if (r < 1 || t[0].type != JSMN_OBJECT) { + SV_INFO("Object expected in HMD configuration\n"); + return -2; + } + + for (i = 1; i < r; i++) { + jsmntok_t * tk = &t[i]; + + char ctxo[100]; + int ilen = tk->end - tk->start; + if( ilen > 99 ) ilen = 99; + memcpy(ctxo, ct0conf + tk->start, ilen); + ctxo[ilen] = 0; + +// printf( "%d / %d / %d / %d %s %d\n", tk->type, tk->start, tk->end, tk->size, ctxo, jsoneq(ct0conf, &t[i], "modelPoints") ); +// printf( "%.*s\n", ilen, ct0conf + tk->start ); + + if (jsoneq(ct0conf, tk, "modelPoints") == 0) { + if( ParsePoints( ctx, so, ct0conf, &so->sensor_locations, t, i ) ) + { + break; + } + } + if (jsoneq(ct0conf, tk, "modelNormals") == 0) { + if( ParsePoints( ctx, so, ct0conf, &so->sensor_normals, t, i ) ) + { + break; + } + } + } + } + else + { + //TODO: Cleanup any remaining USB stuff. + return 1; + } + return 0; +} + + +int survive_vive_close( struct SurviveContext * ctx, void * driver ) +{ + struct SurviveViveData * sv = driver; + + survive_vive_usb_close( sv ); +} + + +int DriverRegHTCVive( struct SurviveContext * ctx ) +{ + int i, r; + struct SurviveObject * hmd = calloc( 1, sizeof( struct SurviveObject ) ); + struct SurviveObject * wm0 = calloc( 1, sizeof( struct SurviveObject ) ); + struct SurviveObject * wm1 = calloc( 1, sizeof( struct SurviveObject ) ); + struct SurviveViveData * sv = calloc( 1, sizeof( struct SurviveViveData ) ); + + sv->ctx = ctx; + + hmd->ctx = ctx; + memcpy( hmd->codename, "HMD", 4 ); + memcpy( hmd->drivername, "HTC", 4 ); + wm0->ctx = ctx; + memcpy( wm0->codename, "WM0", 4 ); + memcpy( wm0->drivername, "HTC", 4 ); + wm1->ctx = ctx; + memcpy( wm1->codename, "WM1", 4 ); + memcpy( wm1->drivername, "HTC", 4 ); + + //USB must happen last. + if( r = survive_usb_init( sv, hmd, wm0, wm1 ) ) + { + //TODO: Cleanup any libUSB stuff sitting around. + goto fail_gracefully; + } + + //Next, pull out the config stuff. + if( LoadConfig( sv, hmd, 1, 0, 0 ) ) goto fail_gracefully; + if( LoadConfig( sv, wm0, 2, 0, 1 ) ) { SV_INFO( "Watchman 0 config issue." ); } + if( LoadConfig( sv, wm1, 3, 0, 1 ) ) { SV_INFO( "Watchman 1 config issue." ); } + + hmd->timebase_hz = wm0->timebase_hz = wm1->timebase_hz = 48000000; + hmd->pulsedist_max_ticks = wm0->pulsedist_max_ticks = wm1->pulsedist_max_ticks = 500000; + hmd->pulselength_min_sync = wm0->pulselength_min_sync = wm1->pulselength_min_sync = 2200; + hmd->pulse_in_clear_time = wm0->pulse_in_clear_time = wm1->pulse_in_clear_time = 35000; + hmd->pulse_max_for_sweep = wm0->pulse_max_for_sweep = wm1->pulse_max_for_sweep = 1800; + + hmd->pulse_synctime_offset = wm0->pulse_synctime_offset = wm1->pulse_synctime_offset = 20000; + hmd->pulse_synctime_slack = wm0->pulse_synctime_slack = wm1->pulse_synctime_slack = 5000; + + hmd->timecenter_ticks = hmd->timebase_hz / 240; + wm0->timecenter_ticks = wm0->timebase_hz / 240; + wm1->timecenter_ticks = wm1->timebase_hz / 240; +/* + int i; + int locs = hmd->nr_locations; + printf( "Locs: %d\n", locs ); + if (hmd->sensor_locations ) + { + printf( "POSITIONS:\n" ); + for( i = 0; i < locs*3; i+=3 ) + { + printf( "%f %f %f\n", hmd->sensor_locations[i+0], hmd->sensor_locations[i+1], hmd->sensor_locations[i+2] ); + } + } + if( hmd->sensor_normals ) + { + printf( "NORMALS:\n" ); + for( i = 0; i < locs*3; i+=3 ) + { + printf( "%f %f %f\n", hmd->sensor_normals[i+0], hmd->sensor_normals[i+1], hmd->sensor_normals[i+2] ); + } + } +*/ + + //Add the drivers. + + survive_add_object( ctx, hmd ); + survive_add_object( ctx, wm0 ); + survive_add_object( ctx, wm1 ); + survive_add_driver( ctx, sv, survive_vive_usb_poll, survive_vive_close, survive_vive_send_magic ); + + return 0; +fail_gracefully: + free( hmd ); + free( wm0 ); + free( wm1 ); + survive_vive_usb_close( sv ); + free( sv ); + return -1; +} + +REGISTER_LINKTIME( DriverRegHTCVive ); + -- cgit v1.2.3 From 373ea323ce2d37b45d438cbe7299365b32240f04 Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Sat, 25 Feb 2017 23:53:33 -0500 Subject: some progress on json file writing --- src/survive_config.c | 214 +++++++++++++++++++++++++++++++++++---------------- src/survive_config.h | 9 ++- 2 files changed, 155 insertions(+), 68 deletions(-) (limited to 'src') diff --git a/src/survive_config.c b/src/survive_config.c index 09153fa..b18f083 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -10,13 +10,15 @@ config_val config_values[MAX_CONFIG_ENTRIES]; static uint16_t used_entries = 0; static FILE *config_file = NULL; +const FLT* config_set_float_a(const char *tag, const FLT* values, uint8_t count); void config_init() { uint16_t i = 0; for (i=0;iBaseStationID); + config_set_float_a("position", bsd->Position, 3); + config_set_float_a("quaternion", bsd->Quaternion, 4); + config_set_float_a("fcalphase", bsd->fcalphase, 2); + config_set_float_a("fcaltilt", bsd->fcaltilt,2); + config_set_float_a("fcalcurve", bsd->fcalcurve,2); + config_set_float_a("fcalgibpha", bsd->fcalgibpha,2); + config_set_float_a("fcalgibmag", bsd->fcalgibmag,2); +} -void sstrcpy(char* dest, const char *src) { +void sstrcpy(char** dest, const char *src) { uint32_t len = strlen(src)+1; - if (dest == NULL) { - dest = (char*)malloc(len); + if (*dest == NULL) { + *dest = (char*)malloc(len); } else { - dest = (char*)realloc(dest, len); + *dest = (char*)realloc(*dest, len); } - strcpy(dest,src); + strcpy(*dest,src); } -const char* config_read_str(const char *tag, const char *value, const char *def_str) { +config_val* find_config_entry(const char *tag) { uint16_t i = 0; for (i=0;idata; + assert(used_entriestag), tag); + sstrcpy(&(cv->data), def_str); + cv->type = CONFIG_STRING; - return config_values[i].str; + return cv->data; } uint32_t config_read_uint32(const char *tag, const uint32_t value, const uint32_t def) { - uint16_t i = 0; - for (i=0;inumeric.i; + assert(used_entriestag), tag); + cv->numeric.i = def; + cv->type = CONFIG_UINT32; - return config_values[i].numeric.i; + return cv->numeric.i; } FLT config_read_float(const char *tag, const FLT value, const FLT def) { - uint16_t i = 0; - for (i=0;inumeric.f; + assert(used_entriestag), tag); + cv->numeric.f = def; + cv->type = CONFIG_FLOAT; - return config_values[i].numeric.f; + return cv->numeric.f; } -const char* config_set_str(const char *tag, const char* value) { - uint16_t i = 0; - +config_val* next_unused_val() { + config_val *cv = config_values+used_entries; assert(used_entriestag), tag); + sstrcpy(&(cv->data), value); + cv->type = CONFIG_STRING; return value; } const uint32_t config_set_uint32(const char *tag, const uint32_t value) { - uint16_t i = 0; - - assert(used_entriestag), tag); + cv->numeric.i = value; + cv->type = CONFIG_UINT32; return value; } const FLT config_set_float(const char *tag, const FLT value) { - uint16_t i = 0; + config_val *cv = find_config_entry(tag); + if (cv == NULL) cv = next_unused_val(); - assert(used_entriestag), tag); + cv->numeric.f = value; + cv->type = CONFIG_FLOAT; return value; } + +const FLT* config_set_float_a(const char *tag, const FLT* values, uint8_t count) { + config_val *cv = find_config_entry(tag); + if (cv == NULL) cv = next_unused_val(); + + sstrcpy(&(cv->tag), tag); + + if (cv->data == NULL) { + cv->data = (char*)malloc(sizeof(FLT)*count); + } + else { + cv->data = (char*)realloc(cv->data, sizeof(FLT)*count); + } + printf("float array\n"); + + memcpy(cv->data,values,sizeof(FLT)*count); + cv->type = CONFIG_FLOAT_ARRAY; + cv->elements = count; + + return values; +} + +void write_float_array(FILE* f, char* tag, FLT* v, uint8_t count) { + uint8_t i = 0; + printf("save float array\n"); + + fprintf(f, "\"%s\":[", tag); + for (i=0;i0) { + fprintf(f, "\"%f\",", v[i]); +// } else { +// fprintf(f, "\"%f\"", v[i]); +// } + } + + fseek(f,-1,SEEK_CUR); + fprintf(f, "]\n"); + +} + void config_save(const char* path) { uint16_t i = 0; @@ -174,7 +256,9 @@ void config_save(const char* path) { } else if (config_values[i].type == CONFIG_UINT32) { fprintf(f, "\"%s\":\"%d\"\n", config_values[i].tag, config_values[i].numeric.i); } else if (config_values[i].type == CONFIG_STRING) { - fprintf(f, "\"%s\":\"%s\"\n", config_values[i].tag, config_values[i].str); + fprintf(f, "\"%s\":\"%s\"\n", config_values[i].tag, config_values[i].data); + } else if (config_values[i].type == CONFIG_FLOAT_ARRAY) { + write_float_array(f, config_values[i].tag, (FLT*)config_values[i].data, config_values[i].elements); } }; diff --git a/src/survive_config.h b/src/survive_config.h index 24762cd..3e62ec8 100644 --- a/src/survive_config.h +++ b/src/survive_config.h @@ -10,7 +10,8 @@ typedef enum { CONFIG_UNKNOWN = 0, CONFIG_FLOAT = 1, CONFIG_UINT32 = 2, - CONFIG_STRING = 3 + CONFIG_STRING = 3, + CONFIG_FLOAT_ARRAY = 4, } cval_type; /* typedef union { @@ -25,13 +26,15 @@ typedef struct { uint32_t i; FLT f; } numeric; - char *str; + char *data; + uint32_t elements; } config_val; - +void config_init(); void config_open(const char* path, const char* mode); void config_close(); void config_write_lighthouse(struct BaseStationData* bsd, uint8_t length); +void config_set_lighthouse(struct BaseStationData* bsd, uint8_t idx); void config_save(const char* path); const FLT config_set_float(const char *tag, const FLT value); -- cgit v1.2.3 From f78983a584768a54503535be0366b07c97738299 Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Sun, 26 Feb 2017 10:20:48 -0500 Subject: use json helpers --- src/survive_config.c | 28 +++++++--------------------- 1 file changed, 7 insertions(+), 21 deletions(-) (limited to 'src') diff --git a/src/survive_config.c b/src/survive_config.c index b18f083..6afe3d1 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -1,8 +1,8 @@ // (C) 2017 <>< Joshua Allen, Under MIT/x11 License. - #include #include #include "survive_config.h" +#include #define MAX_CONFIG_ENTRIES 100 @@ -227,22 +227,8 @@ const FLT* config_set_float_a(const char *tag, const FLT* values, uint8_t count) return values; } -void write_float_array(FILE* f, char* tag, FLT* v, uint8_t count) { - uint8_t i = 0; - printf("save float array\n"); - - fprintf(f, "\"%s\":[", tag); - for (i=0;i0) { - fprintf(f, "\"%f\",", v[i]); -// } else { -// fprintf(f, "\"%f\"", v[i]); -// } - } - - fseek(f,-1,SEEK_CUR); - fprintf(f, "]\n"); - +void _json_write_float_array(FILE* f, const char* tag, FLT* v, uint8_t count) { + json_write_double_array(f,tag,v,count); } void config_save(const char* path) { @@ -252,13 +238,13 @@ void config_save(const char* path) { for (i=0;i<=used_entries;++i) { if (config_values[i].type == CONFIG_FLOAT) { - fprintf(f, "\"%s\":\"%F\"\n", config_values[i].tag, config_values[i].numeric.f); + json_write_float(f, config_values[i].tag, config_values[i].numeric.f); } else if (config_values[i].type == CONFIG_UINT32) { - fprintf(f, "\"%s\":\"%d\"\n", config_values[i].tag, config_values[i].numeric.i); + json_write_uint32(f, config_values[i].tag, config_values[i].numeric.i); } else if (config_values[i].type == CONFIG_STRING) { - fprintf(f, "\"%s\":\"%s\"\n", config_values[i].tag, config_values[i].data); + json_write_str(f, config_values[i].tag, config_values[i].data); } else if (config_values[i].type == CONFIG_FLOAT_ARRAY) { - write_float_array(f, config_values[i].tag, (FLT*)config_values[i].data, config_values[i].elements); + _json_write_float_array(f, config_values[i].tag, (FLT*)config_values[i].data, config_values[i].elements); } }; -- cgit v1.2.3 From 51751d4a9f407f526de2b626d44e14690fbef004 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sun, 26 Feb 2017 10:28:56 -0500 Subject: cleanup --- src/survive.c | 10 +++++++--- src/survive_driverman.c | 6 +++++- src/survive_driverman.h | 20 ++++++++++++++++++-- src/survive_internal.h | 4 ++++ 4 files changed, 34 insertions(+), 6 deletions(-) (limited to 'src') diff --git a/src/survive.c b/src/survive.c index 306dbe1..9bc1a2c 100644 --- a/src/survive.c +++ b/src/survive.c @@ -136,11 +136,15 @@ void survive_close( struct SurviveContext * ctx ) int i; for( i = 0; i < oldct; i++ ) { - ctx->driverpolls[i]( ctx, ctx->drivers[i] ); + ctx->drivercloses[i]( ctx, ctx->drivers[i] ); } - //TODO: Free everything except for self. - //XXX Will leak memory. + free( ctx->objs ); + free( ctx->drivers ); + free( ctx->driverpolls ); + free( ctx->drivermagics ); + free( ctx->drivercloses ); + free( ctx ); } int survive_poll( struct SurviveContext * ctx ) diff --git a/src/survive_driverman.c b/src/survive_driverman.c index b4684c8..8cdfb71 100644 --- a/src/survive_driverman.c +++ b/src/survive_driverman.c @@ -1,3 +1,8 @@ +// (C) 2017 <>< C. N. Lohr, Under MIT/x11 License. +// +// See notice in survive_driverman.h +// + #include "survive_driverman.h" #include #include @@ -45,6 +50,5 @@ void ListDrivers() { printf( " %s\n", DriverNames[i] ); } - } diff --git a/src/survive_driverman.h b/src/survive_driverman.h index fb385da..5e13caf 100644 --- a/src/survive_driverman.h +++ b/src/survive_driverman.h @@ -1,3 +1,12 @@ +// (C) 2017 <>< C. N. Lohr, Under MIT/x11 License. +// +// This file is intended to be used for self-registering functions. By using +// this it means that you do not need to have complicated switch statements or +// #defines for dfferent inclusion of drivers/other code. You can simply +// register your function and it will be put into a list. +// +// + #ifndef SURVIVE_DRIVERMAN_H #define SURVIVE_DRIVERMAN_H @@ -12,11 +21,18 @@ void ListDrivers(); #define REGISTER_LINKTIME( func ) \ void __attribute__((constructor)) Register##func() { RegisterDriver( #func, &func ); } + +// +// Specific types of drivers. +// + struct SurviveContext; +//Device drivers (prefix your drivers with "DriverReg") i.e. +// REGISTER_LINKTIME( DriverRegHTCVive ); typedef int (*DeviceDriver)( struct SurviveContext * ctx ); -typedef int (*DeviceDriverCb)( struct SurviveContext * ctx, void * driver ); -typedef int (*DeviceDriverMagicCb)( struct SurviveContext * ctx, void * driver, int magic_code, void * data, int datalen ); + +//more driver types here? i.e. posefinders, etc. #endif diff --git a/src/survive_internal.h b/src/survive_internal.h index 3dc471a..5962623 100644 --- a/src/survive_internal.h +++ b/src/survive_internal.h @@ -33,6 +33,10 @@ struct SurviveContext; struct SurviveUSBInterface; +typedef int (*DeviceDriverCb)( struct SurviveContext * ctx, void * driver ); +typedef int (*DeviceDriverMagicCb)( struct SurviveContext * ctx, void * driver, int magic_code, void * data, int datalen ); + + //This is defined in survive.h struct SurviveObject; struct SurviveCalData; -- cgit v1.2.3 From e8d696e03128242be33eb0734addee645e894635 Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Mon, 27 Feb 2017 20:52:26 -0500 Subject: basic config can be written to file --- src/survive_cal.c | 5 ++ src/survive_config.c | 230 +++++++++++++++++++++------------------------------ src/survive_config.h | 34 ++++---- 3 files changed, 119 insertions(+), 150 deletions(-) (limited to 'src') diff --git a/src/survive_cal.c b/src/survive_cal.c index 760692c..41d3fb1 100644 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -15,6 +15,8 @@ #include #include +#include "survive_config.h" + #define PTS_BEFORE_COMMON 32 #define NEEDED_COMMON_POINTS 10 #define NEEDED_TIMES_OF_COMMON 5 @@ -49,6 +51,9 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet) b->fcalgibmag[0] = v6.fcal_0_gibmag; b->fcalgibmag[1] = v6.fcal_1_gibmag; b->OOTXSet = 1; + + config_set_lighthouse(b,id); + config_save("config.json"); } int survive_cal_get_status( struct SurviveContext * ctx, char * description, int description_length ) diff --git a/src/survive_config.c b/src/survive_config.c index 6afe3d1..dfbd0e2 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -5,104 +5,65 @@ #include #define MAX_CONFIG_ENTRIES 100 +#define MAX_LIGHTHOUSES 2 -config_val config_values[MAX_CONFIG_ENTRIES]; -static uint16_t used_entries = 0; -static FILE *config_file = NULL; -const FLT* config_set_float_a(const char *tag, const FLT* values, uint8_t count); - -void config_init() { - uint16_t i = 0; - for (i=0;iconfig_entries = malloc(count*sizeof(config_entry)); + cg->used_entries = 0; + cg->max_entries = count; for (i=0;iconfig_entries[i].data = NULL; + cg->config_entries[i].tag = NULL; + cg->config_entries[i].type = CONFIG_UNKNOWN; + cg->config_entries[i].elements = 0; } } -*/ -void set_uint32(char* tag, uint32_t x) { -// fprintf(config_file, "\"%s\":\"%d\"\n", tag, x); +void config_init() { + uint16_t i = 0; + init_config_group(&global_config_values, MAX_CONFIG_ENTRIES); + for(i=0;iBaseStationID); - config_set_float_a("position", bsd->Position, 3); - config_set_float_a("quaternion", bsd->Quaternion, 4); - config_set_float_a("fcalphase", bsd->fcalphase, 2); - config_set_float_a("fcaltilt", bsd->fcaltilt,2); - config_set_float_a("fcalcurve", bsd->fcalcurve,2); - config_set_float_a("fcalgibpha", bsd->fcalgibpha,2); - config_set_float_a("fcalgibmag", bsd->fcalgibmag,2); + config_group *cg = lh_config+idx; + config_set_uint32(cg,"index", idx); + config_set_uint32(cg,"id", bsd->BaseStationID); + config_set_float_a(cg,"position", bsd->Position, 3); + config_set_float_a(cg,"quaternion", bsd->Quaternion, 4); + config_set_float_a(cg,"fcalphase", bsd->fcalphase, 2); + config_set_float_a(cg,"fcaltilt", bsd->fcaltilt,2); + config_set_float_a(cg,"fcalcurve", bsd->fcalcurve,2); + config_set_float_a(cg,"fcalgibpha", bsd->fcalgibpha,2); + config_set_float_a(cg,"fcalgibmag", bsd->fcalgibmag,2); } void sstrcpy(char** dest, const char *src) { uint32_t len = strlen(src)+1; + assert(dest!=NULL); + if (*dest == NULL) { *dest = (char*)malloc(len); } else { @@ -111,71 +72,50 @@ void sstrcpy(char** dest, const char *src) { strcpy(*dest,src); } -config_val* find_config_entry(const char *tag) { +config_entry* find_config_entry(config_group *cg, const char *tag) { uint16_t i = 0; - for (i=0;iused_entries;++i) { + if ( strcmp(cg->config_entries[i].tag, tag) == 0 ) { + return cg->config_entries+i; } } return NULL; } -const char* config_read_str(const char *tag, const char *value, const char *def_str) { - config_val *cv = find_config_entry(tag); +const char* config_read_str(config_group *cg, const char *tag, const char *def) { + config_entry *cv = find_config_entry(cg, tag); if (cv != NULL) return cv->data; - assert(used_entriestag), tag); - sstrcpy(&(cv->data), def_str); - cv->type = CONFIG_STRING; - - return cv->data; + return config_set_str(cg,tag,def); } -uint32_t config_read_uint32(const char *tag, const uint32_t value, const uint32_t def) { - config_val *cv = find_config_entry(tag); +uint32_t config_read_uint32(config_group *cg, const char *tag, const uint32_t def) { + config_entry *cv = find_config_entry(cg, tag); if (cv != NULL) return cv->numeric.i; - assert(used_entriestag), tag); - cv->numeric.i = def; - cv->type = CONFIG_UINT32; - - return cv->numeric.i; + return config_set_uint32(cg, tag, def); } -FLT config_read_float(const char *tag, const FLT value, const FLT def) { - config_val *cv = find_config_entry(tag); +FLT config_read_float(config_group *cg, const char *tag, const FLT def) { + config_entry *cv = find_config_entry(cg, tag); if (cv != NULL) return cv->numeric.f; - assert(used_entriestag), tag); - cv->numeric.f = def; - cv->type = CONFIG_FLOAT; - - return cv->numeric.f; + config_set_float(cg, tag, def); } -config_val* next_unused_val() { - config_val *cv = config_values+used_entries; - assert(used_entriesconfig_entries + cg->used_entries; + assert(cg->used_entries < cg->max_entries); + cg->used_entries++; return cv; } -const char* config_set_str(const char *tag, const char* value) { - config_val *cv = find_config_entry(tag); - if (cv == NULL) cv = next_unused_val(); +const char* config_set_str(config_group *cg, const char *tag, const char* value) { + config_entry *cv = find_config_entry(cg, tag); + if (cv == NULL) cv = next_unused_entry(cg); sstrcpy(&(cv->tag), tag); sstrcpy(&(cv->data), value); @@ -184,9 +124,9 @@ const char* config_set_str(const char *tag, const char* value) { return value; } -const uint32_t config_set_uint32(const char *tag, const uint32_t value) { - config_val *cv = find_config_entry(tag); - if (cv == NULL) cv = next_unused_val(); +const uint32_t config_set_uint32(config_group *cg, const char *tag, const uint32_t value) { + config_entry *cv = find_config_entry(cg, tag); + if (cv == NULL) cv = next_unused_entry(cg); sstrcpy(&(cv->tag), tag); cv->numeric.i = value; @@ -195,9 +135,9 @@ const uint32_t config_set_uint32(const char *tag, const uint32_t value) { return value; } -const FLT config_set_float(const char *tag, const FLT value) { - config_val *cv = find_config_entry(tag); - if (cv == NULL) cv = next_unused_val(); +const FLT config_set_float(config_group *cg, const char *tag, const FLT value) { + config_entry *cv = find_config_entry(cg, tag); + if (cv == NULL) cv = next_unused_entry(cg); sstrcpy(&(cv->tag), tag); cv->numeric.f = value; @@ -206,9 +146,9 @@ const FLT config_set_float(const char *tag, const FLT value) { return value; } -const FLT* config_set_float_a(const char *tag, const FLT* values, uint8_t count) { - config_val *cv = find_config_entry(tag); - if (cv == NULL) cv = next_unused_val(); +const FLT* config_set_float_a(config_group *cg, const char *tag, const FLT* values, uint8_t count) { + config_entry *cv = find_config_entry(cg, tag); + if (cv == NULL) cv = next_unused_entry(cg); sstrcpy(&(cv->tag), tag); @@ -231,23 +171,41 @@ void _json_write_float_array(FILE* f, const char* tag, FLT* v, uint8_t count) { json_write_double_array(f,tag,v,count); } -void config_save(const char* path) { +void write_config_group(FILE* f, config_group *cg, char *tag) { uint16_t i = 0; - FILE* f = fopen(path, "w"); + if (tag != NULL) { + fprintf(f, "\"%s\":{\n", tag); + } - for (i=0;i<=used_entries;++i) { - if (config_values[i].type == CONFIG_FLOAT) { - json_write_float(f, config_values[i].tag, config_values[i].numeric.f); - } else if (config_values[i].type == CONFIG_UINT32) { - json_write_uint32(f, config_values[i].tag, config_values[i].numeric.i); - } else if (config_values[i].type == CONFIG_STRING) { - json_write_str(f, config_values[i].tag, config_values[i].data); - } else if (config_values[i].type == CONFIG_FLOAT_ARRAY) { - _json_write_float_array(f, config_values[i].tag, (FLT*)config_values[i].data, config_values[i].elements); + for (i=0;i < cg->used_entries;++i) { + if (cg->config_entries[i].type == CONFIG_FLOAT) { + json_write_float(f, cg->config_entries[i].tag, cg->config_entries[i].numeric.f); + } else if (cg->config_entries[i].type == CONFIG_UINT32) { + json_write_uint32(f, cg->config_entries[i].tag, cg->config_entries[i].numeric.i); + } else if (cg->config_entries[i].type == CONFIG_STRING) { + json_write_str(f, cg->config_entries[i].tag, cg->config_entries[i].data); + } else if (cg->config_entries[i].type == CONFIG_FLOAT_ARRAY) { + _json_write_float_array(f, cg->config_entries[i].tag, (FLT*)cg->config_entries[i].data, cg->config_entries[i].elements); } + if ((i+1) < cg->used_entries) fprintf(f,","); + fprintf(f,"\n"); }; + if (tag != NULL) { + fprintf(f,"}\n"); + } +} + +void config_save(const char* path) { + uint16_t i = 0; + + FILE* f = fopen(path, "w"); + + write_config_group(f,&global_config_values, NULL); + write_config_group(f,lh_config, "lighthouse0"); + write_config_group(f,lh_config+1, "lighthouse1"); + fclose(f); } diff --git a/src/survive_config.h b/src/survive_config.h index 3e62ec8..14e2fc6 100644 --- a/src/survive_config.h +++ b/src/survive_config.h @@ -13,12 +13,8 @@ typedef enum { CONFIG_STRING = 3, CONFIG_FLOAT_ARRAY = 4, } cval_type; -/* -typedef union { - uint32_t i; - FLT f; - } Numeric; -*/ + + typedef struct { char *tag; cval_type type; @@ -28,21 +24,31 @@ typedef struct { } numeric; char *data; uint32_t elements; -} config_val; +} config_entry; + +typedef struct { + config_entry *config_entries; + uint16_t used_entries; + uint16_t max_entries; +} config_group; + +extern config_group global_config_values; +extern config_group lh_config[2]; //lighthouse configs + void config_init(); void config_open(const char* path, const char* mode); void config_close(); -void config_write_lighthouse(struct BaseStationData* bsd, uint8_t length); +//void config_write_lighthouse(struct BaseStationData* bsd, uint8_t length); void config_set_lighthouse(struct BaseStationData* bsd, uint8_t idx); void config_save(const char* path); -const FLT config_set_float(const char *tag, const FLT value); -const uint32_t config_set_uint32(const char *tag, const uint32_t value); -const char* config_set_str(const char *tag, const char* value); -FLT config_read_float(const char *tag, const FLT value, const FLT def); +const FLT config_set_float(config_group *cg, const char *tag, const FLT value); +const uint32_t config_set_uint32(config_group *cg, const char *tag, const uint32_t value); +const char* config_set_str(config_group *cg, const char *tag, const char* value); -uint32_t config_read_uint32(const char *tag, const uint32_t value, const uint32_t def); -const char* config_read_str(const char *tag, const char *value, const char *def_str); +FLT config_read_float(config_group *cg, const char *tag, const FLT def); +uint32_t config_read_uint32(config_group *cg, const char *tag, const uint32_t def); +const char* config_read_str(config_group *cg, const char *tag, const char *def); #endif \ No newline at end of file -- cgit v1.2.3 From 77644ff26ee7b8fcef3997d0ffe183d0a80c85e4 Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Mon, 27 Feb 2017 21:52:39 -0500 Subject: return set float value --- src/survive_config.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/survive_config.c b/src/survive_config.c index dfbd0e2..2ed265b 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -103,7 +103,7 @@ FLT config_read_float(config_group *cg, const char *tag, const FLT def) { if (cv != NULL) return cv->numeric.f; - config_set_float(cg, tag, def); + return config_set_float(cg, tag, def); } config_entry* next_unused_entry(config_group *cg) { -- cgit v1.2.3 From b0dc9c89146d2a5904a4ecc6b9c1f8d1c5fcfce0 Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Mon, 27 Feb 2017 22:00:35 -0500 Subject: check for correct float size --- src/survive_config.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/survive_config.c b/src/survive_config.c index 2ed265b..5531006 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -168,7 +168,11 @@ const FLT* config_set_float_a(config_group *cg, const char *tag, const FLT* valu } void _json_write_float_array(FILE* f, const char* tag, FLT* v, uint8_t count) { - json_write_double_array(f,tag,v,count); + if (sizeof(FLT) == sizeof(double)) { + json_write_double_array(f,tag,v,count); + } else if (sizeof(FLT) == sizeof(float)) { + json_write_double_array(f,tag,v,count); + } } void write_config_group(FILE* f, config_group *cg, char *tag) { -- cgit v1.2.3 From 6cc609204ba7ea1bd0a903cf918a2b7eea979b91 Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Mon, 27 Feb 2017 22:08:36 -0500 Subject: compile time check of float type --- src/survive_config.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/survive_config.c b/src/survive_config.c index 5531006..c46e300 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -168,11 +168,11 @@ const FLT* config_set_float_a(config_group *cg, const char *tag, const FLT* valu } void _json_write_float_array(FILE* f, const char* tag, FLT* v, uint8_t count) { - if (sizeof(FLT) == sizeof(double)) { + #ifdef USE_DOUBLE json_write_double_array(f,tag,v,count); - } else if (sizeof(FLT) == sizeof(float)) { - json_write_double_array(f,tag,v,count); - } + #else + json_write_float_array(f,tag,v,count); + #endif } void write_config_group(FILE* f, config_group *cg, char *tag) { -- cgit v1.2.3 From 9d1b1d09ed51344c8ca7b4f0a94f5841ee2c509e Mon Sep 17 00:00:00 2001 From: cnlohr Date: Thu, 2 Mar 2017 22:20:07 -0500 Subject: cleanup, add some logging data to calinfo, open up restrictive rules about sensor positions, clean cleans redist/ --- src/survive_cal_lhfind.c | 2 +- src/survive_vive.c | 22 ++++++++++++++++++++++ 2 files changed, 23 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/survive_cal_lhfind.c b/src/survive_cal_lhfind.c index e1e5fc9..93d9dc0 100644 --- a/src/survive_cal_lhfind.c +++ b/src/survive_cal_lhfind.c @@ -129,7 +129,7 @@ int survive_cal_lhfind( struct SurviveCalData * cd ) fullrange *= 0.25; } - if( beste > 0.005 ) + if( beste > 0.01 ) { //Error too high SV_ERROR( "LH: %d / Best E %f Error too high\n", lh, beste ); diff --git a/src/survive_vive.c b/src/survive_vive.c index a402153..47886c9 100644 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -20,6 +20,7 @@ #include //sleep if I ever use it. #include #include +#include struct SurviveViveData; @@ -976,6 +977,27 @@ static int LoadConfig( struct SurviveViveData * sv, struct SurviveObject * so, i //TODO: Cleanup any remaining USB stuff. return 1; } + + char fname[20]; + mkdir( "calinfo", 0755 ); + + sprintf( fname, "calinfo/%s_points.csv", so->codename ); + FILE * f = fopen( fname, "w" ); + int j; + for( j = 0; j < so->nr_locations; j++ ) + { + fprintf( f, "%f %f %f\n", so->sensor_locations[j*3+0], so->sensor_locations[j*3+1], so->sensor_locations[j*3+2] ); + } + fclose( f ); + + sprintf( fname, "calinfo/%s_normals.csv", so->codename ); + f = fopen( fname, "w" ); + for( j = 0; j < so->nr_locations; j++ ) + { + fprintf( f, "%f %f %f\n", so->sensor_normals[j*3+0], so->sensor_normals[j*3+1], so->sensor_normals[j*3+2] ); + } + fclose( f ); + return 0; } -- cgit v1.2.3