diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/poser_daveortho.c | 85 | ||||
-rwxr-xr-x | src/survive_cal.c | 59 | ||||
-rw-r--r-- | src/survive_cal.h | 7 | ||||
-rw-r--r-- | src/survive_data.c | 57 | ||||
-rw-r--r-- | src/survive_process.c | 3 | ||||
-rwxr-xr-x | src/survive_vive.c | 51 |
6 files changed, 215 insertions, 47 deletions
diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c index e81e154..80f65a9 100644 --- a/src/poser_daveortho.c +++ b/src/poser_daveortho.c @@ -265,7 +265,8 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); // FLT ydist1 = 1.0 / uhat_len; //0.25*PI / uhat_len; // FLT ydist2 = 1.0 / rhat_len; //0.25*PI / rhat_len; FLT ydist = 1.0 / urhat_len; - //printf("ydist1 %f ydist2 %f ydist %f\n", ydist1, ydist2, ydist); + printf("ydist %f\n", ydist); +// printf("ydist1 %f ydist2 %f ydist %f\n", ydist1, ydist2, ydist); //-------------------- // Rescale the axies to be of the proper length @@ -282,7 +283,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); if( x_y != x_y ) x_y = 0; if( y_y != y_y ) y_y = 0; if( z_y != z_y ) z_y = 0; -/* + // Exhaustively flip the minus sign of the z axis until we find the right one . . . FLT bestErr = 9999.0; FLT xy_dot2 = x[0][0]*y[0][0] + x[2][0]*y[2][0]; @@ -302,7 +303,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); FLT cx,cy,cz; CrossProduct(cx,cy,cz,x[0][0],x_y,x[2][0],y[0][0],y_y,y[2][0]); FLT hand = cx*z[0][0] + cy*z_y + cz*z[2][0]; - printf("err %f hand %f\n", err, hand); +// printf("err %f hand %f\n", err, hand); // If we are the best right-handed frame so far //if (hand > 0 && err < bestErr) { x[1][0]=x_y; y[1][0]=y_y; z[1][0]=z_y; bestErr=err; } @@ -313,9 +314,9 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); } x_y = -x_y; } - printf("bestErr %f\n", bestErr); -*/ +// printf("bestErr %f\n", bestErr); +/* //------------------------- // A test version of the rescaling to the proper length //------------------------- @@ -338,7 +339,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); if( y_y != y_y ) y_y = 0; if( z_y != z_y ) z_y = 0; - printf( "---> %f %f %f\n", x_y, y_y, z_y ); +// printf( "---> %f %f %f\n", x_y, y_y, z_y ); // Exhaustively flip the minus sign of the z axis until we find the right one . . . FLT bestErr = 9999.0; @@ -359,7 +360,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); FLT cx,cy,cz; CrossProduct(cx,cy,cz,x2[0][0],x_y,x2[2][0],y2[0][0],y_y,y2[2][0]); FLT hand = cx*z2[0][0] + cy*z_y + cz*z2[2][0]; - printf("err %f hand %f\n", err, hand); + //printf("err %f hand %f\n", err, hand); // If we are the best right-handed frame so far if (hand > 0 && err < bestErr) { x2[1][0]=x_y; y2[1][0]=y_y; z2[1][0]=z_y; bestErr=err; } @@ -380,7 +381,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); } } ydist = bestYdist; - +*/ /* for (i=0; i<nPoints; i++) { FLT x1 = x[0][0]*X[0][i] + y[0][0]*X[1][i] + z[0][0]*X[2][i]; @@ -441,7 +442,71 @@ PRINT(ab,2,1); T[2][0]=R[2][0]; T[2][1]=R[2][1]; T[2][2]=R[2][2]; T[2][3]=trans[2]; T[3][0]=0.0; T[3][1]=0.0; T[3][2]=0.0; T[3][3]=1.0; - PRINT_MAT(T,4,4); + + FLT T2[4][4]; + + //------------------- + // Orthogonalize the matrix + //------------------- + FLT temp[4][4]; + FLT quat[4], quatNorm[4]; + FLT euler[3]; + + + //------------------- + // Orthogonalize the matrix + //------------------- + + PRINT_MAT(T,4,4); +matrix44transpose(T2, T); +//matrix44copy(T2,T); + cross3d( &T2[0][0], &T2[1][0], &T2[2][0] ); + cross3d( &T2[2][0], &T2[0][0], &T2[1][0] ); + normalize3d( &T2[0][0], &T2[0][0] ); + normalize3d( &T2[1][0], &T2[1][0] ); + normalize3d( &T2[2][0], &T2[2][0] ); +//matrix44copy(T2,T); +matrix44transpose(T, T2); + PRINT_MAT(T,4,4); + + + + //memcpy(T2,T,16*sizeof(float)); +// matrix44copy(T2,T); + matrix44transpose(T2,T); + quatfrommatrix( quat, &T2[0][0] ); + quatnormalize(quatNorm,quat); + quattoeuler(euler,quatNorm); + quattomatrix( T2, quatNorm ); + printf("rot %f %f %f len %f\n", euler[0], euler[1], euler[2], quatmagnitude(quat)); + PRINT(T,4,4); + +// matrix44copy(temp,T2); + matrix44transpose(temp,T2); + + memcpy(T2,temp,16*sizeof(float)); + for (i=0; i<3; i++) { + for (j=0; j<3; j++) { + T[i][j] = T2[j][i]; + } + } + PRINT(T2,4,4); + + +/* + CrossProduct(T[0][2],T[1][2],T[2][2], T[0][0],T[1][0],T[2][0], T[0][1],T[1][1],T[2][1]); + CrossProduct(T[0][0],T[1][0],T[2][0], T[0][1],T[1][1],T[2][1], T[0][2],T[1][2],T[2][2]); + CrossProduct(T[0][1],T[1][1],T[2][1], T[0][2],T[1][2],T[2][2], T[0][0],T[1][0],T[2][0]); + float xlen = sqrt(T[0][0]*T[0][0] + T[1][0]*T[1][0] + T[2][0]*T[2][0]); + float ylen = sqrt(T[0][1]*T[0][1] + T[1][1]*T[1][1] + T[2][1]*T[2][1]); + float zlen = sqrt(T[0][2]*T[0][2] + T[1][0]*T[1][2] + T[2][2]*T[2][2]); + T[0][0]/=xlen; T[1][0]/=xlen; T[2][0]/=xlen; + T[0][1]/=ylen; T[1][1]/=ylen; T[2][1]/=ylen; + T[0][2]/=zlen; T[1][2]/=zlen; T[2][2]/=zlen; +*/ + + + // PRINT_MAT(T,4,4); //------------------- // Plot the output points //------------------- @@ -453,7 +518,7 @@ PRINT(ab,2,1); S_out[1][i] = atan2(Tz, Ty); // vert //S_out[0][i] = Tx; //S_out[1][i] = Tz; - printf("point %i Txyz %f %f %f in %f %f out %f %f morph %f %f\n", i, Tx,Ty,Tz, S_in[0][i], S_in[1][i], S_out[0][i], S_out[1][i], S_morph[0][i], S_morph[1][i]); +// printf("point %i Txyz %f %f %f in %f %f out %f %f morph %f %f\n", i, Tx,Ty,Tz, S_in[0][i], S_in[1][i], S_out[0][i], S_out[1][i], S_morph[0][i], S_morph[1][i]); } } diff --git a/src/survive_cal.c b/src/survive_cal.c index dfa2e85..19eb3ca 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -223,6 +223,21 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int //cd->stage = 2; //If all lighthouses have their OOTX set, move on. } break; + case 3: //Look for light sync lengths. + { + if( acode >= -2 ) break; + else if( acode < -4 ) break; + int lh = (-acode) - 3; + + if( strcmp( so->codename, "WM0" ) == 0 ) + sensor_id += 32; + if( strcmp( so->codename, "WM1" ) == 0 ) + sensor_id += 64; + + cd->all_sync_times[sensor_id][lh][cd->all_sync_counts[sensor_id][lh]++] = length; + break; + } + } } @@ -299,6 +314,7 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin if( sensors_visible < MIN_SENSORS_VISIBLE_PER_LH_FOR_CAL ) { //printf( "Dev %d, LH %d not enough visible points found.\n", i, j ); + reset_calibration( cd ); cd->found_common = 0; return; } @@ -359,6 +375,8 @@ static void reset_calibration( struct SurviveCalData * cd ) cd->found_common = 0; cd->times_found_common = 0; cd->stage = 2; + + memset( cd->all_sync_counts, 0, sizeof( cd->all_sync_counts ) ); } static void handle_calibration( struct SurviveCalData *cd ) @@ -384,9 +402,45 @@ static void handle_calibration( struct SurviveCalData *cd ) #else mkdir( "calinfo", 0755 ); #endif + int sen, axis, lh; + FLT temp_syncs[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES]; + + //Just to get it out of the way early, we'll calculate the sync-pulse-lengths here. + FILE * sync_time_info = fopen( "calinfo/synctime.csv", "w" ); + + for( sen = 0; sen < MAX_SENSORS_TO_CAL; sen++ ) + for( lh = 0; lh < NUM_LIGHTHOUSES; lh++ ) + { + int count = cd->all_sync_counts[sen][lh]; + int i; + double totaltime; + + totaltime = 0; + + if( count < 20 ) continue; + for( i = 0; i < count; i++ ) + { + totaltime += cd->all_sync_times[sen][lh][i]; + } + FLT avg = totaltime/count; + + double stddev = 0.0; + for( i = 0; i < count; i++ ) + { + stddev += (cd->all_sync_times[sen][lh][i] - avg)*(cd->all_sync_times[sen][lh][i] - avg); + } + stddev /= count; + + fprintf( sync_time_info, "%d %d %f %d %f\n", sen, lh, totaltime/count, count, stddev ); + } + + fclose( sync_time_info ); + + + + 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++ ) @@ -467,7 +521,7 @@ static void handle_calibration( struct SurviveCalData *cd ) FLT stddevlen = 0; #define HISTOGRAMSIZE 31 - #define HISTOGRAMBINANG 0.00001 //TODO: Tune + #define HISTOGRAMBINANG ((3.14159)/400000.0) //TODO: Tune int histo[HISTOGRAMSIZE]; memset( histo, 0, sizeof( histo ) ); @@ -552,6 +606,7 @@ static void handle_calibration( struct SurviveCalData *cd ) fsd.lengths[i][j][1] = cd->avglens[dataindex+1]; fsd.angles[i][j][0] = cd->avgsweeps[dataindex+0]; fsd.angles[i][j][1] = cd->avgsweeps[dataindex+1]; + fsd.synctimes[i][j] = temp_syncs[i][j]; } int r = cd->ConfigPoserFn( cd->poseobjects[obj], (PoserData*)&fsd ); diff --git a/src/survive_cal.h b/src/survive_cal.h index 9d7b3a9..8f4e4de 100644 --- a/src/survive_cal.h +++ b/src/survive_cal.h @@ -35,7 +35,9 @@ void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t t #define MAX_SENSORS_TO_CAL 96 #define MIN_PTS_BEFORE_CAL 24 -#define DRPTS 128 + + +#define DRPTS 32 //Number of samples required in collection phase. #define MAX_POSE_OBJECTS 10 @@ -54,6 +56,9 @@ struct SurviveCalData int8_t found_common; int8_t times_found_common; + FLT all_sync_times[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES][DRPTS]; + int16_t all_sync_counts[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES]; + //For camfind (4+) //Index is calculated with: int dataindex = sen*(2*NUM_LIGHTHOUSES)+lh*2+axis; FLT avgsweeps[MAX_CAL_PT_DAT]; diff --git a/src/survive_data.c b/src/survive_data.c index 00e66f0..ee180b1 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -255,24 +255,47 @@ void handle_lightcap2( SurviveObject * so, LightcapElement * le ) } +int32_t decode_acode(uint32_t length, int32_t main_divisor) { + //+50 adds a small offset and seems to help always get it right. + //Check the +50 in the future to see how well this works on a variety of hardware. + + int32_t acode = (length+main_divisor+50)/(main_divisor*2); + if( acode & 1 ) return -1; + + return (acode>>1) - 6; +} //This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode. void handle_lightcap( SurviveObject * so, LightcapElement * le ) { - handle_lightcap2(so,le); - return; - SurviveContext * ctx = so->ctx; - //int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time; - - //if( so->codename[0] != 'H' ) +// handle_lightcap2(so,le); +// return; + //int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time; if( le->sensor_id > SENSORS_PER_OBJECT ) { return; } +#if 0 + if( so->codename[0] == 'H' ) + { + static int lt; + static int last; + if( le->length > 1000 ) + { + int dl = le->timestamp - lt; + lt = le->timestamp; + if( dl > 10000 || dl < -10000 ) + printf( "+++%s %3d %5d %9d ", so->codename, le->sensor_id, le->length, dl ); + if( dl > 100000 ) printf(" \n" ); + } + last=le->length; + } +#endif + so->tsl = le->timestamp; if( le->length < 20 ) return; ///Assuming 20 is an okay value for here. @@ -298,7 +321,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) { int is_new_pulse = delta > so->pulselength_min_sync /*1500*/ + last_sync_length; - //printf("m sync %d %d %d %d\n", le->sensor_id, so->last_sync_time[ssn], le->timestamp, delta); + so->did_handle_ootx = 0; @@ -343,6 +366,14 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) } } } + + //Extra tidbit for storing length-of-sync-pulses. + { + int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz. + int base_station = is_new_pulse; + //printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length ); + ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length ); + } } @@ -366,17 +397,13 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) int32_t acode_array[2] = { - (so->last_sync_length[0]+main_divisor+50)/(main_divisor*2), //+50 adds a small offset and seems to help always get it right. - (so->last_sync_length[1]+main_divisor+50)/(main_divisor*2), //Check the +50 in the future to see how well this works on a variety of hardware. + decode_acode(so->last_sync_length[0],main_divisor), + decode_acode(so->last_sync_length[1],main_divisor) }; //XXX: TODO: Capture error count here. - if( acode_array[0] & 1 ) return; - if( acode_array[1] & 1 ) return; - - acode_array[0] = (acode_array[0]>>1) - 6; - acode_array[1] = (acode_array[1]>>1) - 6; - + if( acode_array[0] < 0 ) return; + if( acode_array[1] < 0 ) return; int acode = acode_array[0]; diff --git a/src/survive_process.c b/src/survive_process.c index 463481a..d4604d8 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -16,6 +16,9 @@ void survive_default_light_process( SurviveObject * so, int sensor_id, int acode survive_cal_light( so, sensor_id, acode, timeinsweep, timecode, length, lh); } + //We don't use sync times, yet. + if( acode < -1 ) return; + if( base_station > NUM_LIGHTHOUSES ) return; //No loner need sync information past this point. diff --git a/src/survive_vive.c b/src/survive_vive.c index c04fc03..f6465b2 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -141,10 +141,10 @@ void survive_data_cb( SurviveUSBInterface * si ); //USB Subsystem void survive_usb_close( SurviveContext * t ); -int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *wm0, SurviveObject * wm1, SurviveObject * tr0, struct SurviveObject * ww0 ); +int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *wm0, SurviveObject * wm1, SurviveObject * tr0 , SurviveObject * ww0 ); int survive_usb_poll( SurviveContext * ctx ); int survive_get_config( char ** config, SurviveViveData * ctx, int devno, int iface, int send_extra_magic ); -int survive_vive_send_magic(struct SurviveContext * ctx, void * drv, int magic_code, void * data, int datalen ); +int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, void * data, int datalen ); #ifdef HIDAPI void * HAPIReceiver( void * v ) @@ -177,8 +177,8 @@ void * HAPIReceiver( void * v ) #else static void handle_transfer(struct libusb_transfer* transfer) { - struct SurviveUSBInterface * iface = transfer->user_data; - struct SurviveContext * ctx = iface->ctx; + SurviveUSBInterface * iface = transfer->user_data; + SurviveContext * ctx = iface->ctx; if( transfer->status != LIBUSB_TRANSFER_COMPLETED ) { @@ -307,9 +307,9 @@ static inline int hid_get_feature_report_timeout(USBHANDLE device, uint16_t ifac return -1; } -int survive_usb_init( struct SurviveViveData * sv, struct SurviveObject * hmd, struct SurviveObject *wm0, struct SurviveObject * wm1, struct SurviveObject * tr0 , struct SurviveObject * ww0 ) +int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *wm0, SurviveObject * wm1, SurviveObject * tr0 , SurviveObject * ww0 ) { - struct SurviveContext * ctx = sv->ctx; + SurviveContext * ctx = sv->ctx; #ifdef HIDAPI if( !GlobalRXUSBMutx ) { @@ -488,10 +488,10 @@ int survive_usb_init( struct SurviveViveData * sv, struct SurviveObject * hmd, s return 0; } -int survive_vive_send_magic(struct SurviveContext * ctx, void * drv, int magic_code, void * data, int datalen ) +int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, void * data, int datalen ) { int r; - struct SurviveViveData * sv = drv; + SurviveViveData * sv = drv; printf( "*CALLING %p %p\n", ctx, sv ); //XXX TODO: Handle haptics, etc. @@ -583,7 +583,7 @@ int survive_vive_send_magic(struct SurviveContext * ctx, void * drv, int magic_c return 0; } -void survive_vive_usb_close( struct SurviveViveData * sv ) +void survive_vive_usb_close( SurviveViveData * sv ) { int i; #ifdef HIDAPI @@ -608,7 +608,7 @@ void survive_vive_usb_close( struct SurviveViveData * sv ) #endif } -int survive_vive_usb_poll( struct SurviveContext * ctx, void * v ) +int survive_vive_usb_poll( SurviveContext * ctx, void * v ) { #ifdef HIDAPI OGUnlockMutex( GlobalRXUSBMutx ); @@ -616,11 +616,11 @@ int survive_vive_usb_poll( struct SurviveContext * ctx, void * v ) OGUnlockMutex( GlobalRXUSBMutx ); return 0; #else - struct SurviveViveData * sv = v; + SurviveViveData * sv = v; int r = libusb_handle_events( sv->usbctx ); if( r ) { - struct SurviveContext * ctx = sv->ctx; + SurviveContext * ctx = sv->ctx; SV_ERROR( "Libusb poll failed." ); } return r; @@ -629,9 +629,9 @@ int survive_vive_usb_poll( struct SurviveContext * ctx, void * v ) } -int survive_get_config( char ** config, struct SurviveViveData * sv, int devno, int iface, int send_extra_magic ) +int survive_get_config( char ** config, SurviveViveData * sv, int devno, int iface, int send_extra_magic ) { - struct SurviveContext * ctx = sv->ctx; + SurviveContext * ctx = sv->ctx; int ret, count = 0, size = 0; uint8_t cfgbuff[64]; uint8_t compressed_data[8192]; @@ -749,7 +749,7 @@ int survive_get_config( char ** config, struct SurviveViveData * sv, int devno, #define POP2 (*(((uint16_t*)((readdata+=2)-2)))) #define POP4 (*(((uint32_t*)((readdata+=4)-4)))) -static void handle_watchman( struct SurviveObject * w, uint8_t * readdata ) +static void handle_watchman( SurviveObject * w, uint8_t * readdata ) { uint8_t startread[29]; @@ -771,7 +771,7 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata ) qty-=2; int propset = 0; int doimu = 0; - + int i; if( (type & 0xf0) == 0xf0 ) { @@ -916,11 +916,12 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata ) LightcapElement les[10]; int lese = 0; //les's end + //Second, go through all LEDs and extract the lightevent from them. { uint8_t *marked; marked = alloca(nrtime); - memset( marked, 0, sizeof( marked ) ); + memset( marked, 0, nrtime ); int i, parpl = 0; timecount--; int timepl = 0; @@ -933,8 +934,20 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata ) led >>= 3; while( marked[timepl] ) timepl++; + +#ifdef DEBUG_WATCHMAN + int i; + printf( "TP %d TC: %d : ", timepl, timecount ); + for( i = 0; i < nrtime; i++ ) + { + printf( "%d", marked[i] ); + } + printf( "\n" ); +#endif + 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. @@ -978,7 +991,7 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata ) end: { SurviveContext * ctx = w->ctx; - SV_INFO( "Light decoding fault: %d\n", fault ); + SV_INFO( "Light decoding fault: %d", fault ); } } } @@ -1015,7 +1028,7 @@ void survive_data_cb( SurviveUSBInterface * si ) { case USB_IF_HMD: { - struct SurviveObject * headset = obj; + SurviveObject * headset = obj; readdata+=2; headset->buttonmask = POP1; //Lens headset->axis2 = POP2; //Lens Separation |