From a015a5bc3631f813281c5af85a29b9d84a3eb924 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Tue, 19 Dec 2017 23:30:52 -0700 Subject: Add support for using only 1 lighthouse Change adds a config option for the number of "active" lighthouses, which can be 1. Calibrate will run with a single lighthouse if the new config option LighthouseCount is set to 1. --- calibrate.c | 2 ++ include/libsurvive/survive.h | 1 + src/survive.c | 2 ++ src/survive_cal.c | 4 ++-- 4 files changed, 7 insertions(+), 2 deletions(-) diff --git a/calibrate.c b/calibrate.c index 43643b3..8c56e43 100644 --- a/calibrate.c +++ b/calibrate.c @@ -14,6 +14,7 @@ struct SurviveContext * ctx; int quit = 0; +static LighthouseCount = 0; void HandleKey( int keycode, int bDown ) { @@ -343,6 +344,7 @@ void * SurviveThread(void *jnk) { ctx = survive_init( 0 ); + uint8_t i =0; for (i=0;i<32;++i) { sensor_name[i] = malloc(3); diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 1532d62..c4b2abe 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -102,6 +102,7 @@ struct SurviveContext struct config_group* lh_config; //lighthouse configs //Calibration data: + int activeLighthouses; BaseStationData bsd[NUM_LIGHTHOUSES]; SurviveCalData * calptr; //If and only if the calibration subsystem is attached. diff --git a/src/survive.c b/src/survive.c index f637bd8..0c88137 100755 --- a/src/survive.c +++ b/src/survive.c @@ -75,6 +75,8 @@ SurviveContext * survive_init( int headless ) config_read(ctx, "config.json"); + ctx->activeLighthouses = config_read_uint32(ctx->global_config_values, "LighthouseCount", 2); + ctx->faultfunction = survivefault; ctx->notefunction = survivenote; diff --git a/src/survive_cal.c b/src/survive_cal.c index ae92bad..0dbadd3 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -218,7 +218,7 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int int i; for( i = 0; i < NUM_LIGHTHOUSES; i++ ) if( ctx->bsd[i].OOTXSet == 0 ) break; - if( i == NUM_LIGHTHOUSES ) cd->stage = 2; //TODO: Make this configuratble to allow single lighthouse. + if( i == ctx->activeLighthouses ) cd->stage = 2; //TODO: Make this configuratble to allow single lighthouse. } break; case 3: //Look for light sync lengths. @@ -309,7 +309,7 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin cd->found_common = 1; for( i = 0; i < cd->numPoseObjects; i++ ) //for( i = 0; i < MAX_SENSORS_TO_CAL/SENSORS_PER_OBJECT; i++ ) - for( j = 0; j < NUM_LIGHTHOUSES; j++ ) + for( j = 0; j < ctx->activeLighthouses; j++ ) { int sensors_visible = 0; for( k = 0; k < SENSORS_PER_OBJECT; k++ ) -- cgit v1.2.3 From a9739c252fab9b32684148625b08b2cfb89a3751 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Wed, 20 Dec 2017 18:19:03 -0700 Subject: Save Calibration Output To Config --- src/poser_turveytori.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 80e8d89..d0b9ab8 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1732,6 +1732,10 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) // } //} + config_set_lighthouse(ctx->lh_config, ctx->bsd[0], 0); + config_set_lighthouse(ctx->lh_config, ctx->bsd[1], 1); + + config_save(ctx, "config.json"); free(to); //printf( "Full scene data.\n" ); -- cgit v1.2.3 From 9dfa5463a83d5219b4947a36500ae097101b6912 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Wed, 20 Dec 2017 20:00:55 -0700 Subject: Load LH Config Data --- src/survive.c | 2 ++ src/survive_config.c | 52 +++++++++++++++++++++++++++++++++++++++++++++++++--- src/survive_config.h | 3 ++- 3 files changed, 53 insertions(+), 4 deletions(-) diff --git a/src/survive.c b/src/survive.c index 0c88137..057462f 100755 --- a/src/survive.c +++ b/src/survive.c @@ -76,6 +76,8 @@ SurviveContext * survive_init( int headless ) config_read(ctx, "config.json"); ctx->activeLighthouses = config_read_uint32(ctx->global_config_values, "LighthouseCount", 2); + config_read_lighthouse(ctx->lh_config, &(ctx->bsd[0]), 0); + config_read_lighthouse(ctx->lh_config, &(ctx->bsd[1]), 1); ctx->faultfunction = survivefault; ctx->notefunction = survivenote; diff --git a/src/survive_config.c b/src/survive_config.c index 3a83902..a24364f 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -86,6 +86,42 @@ void config_init() { } */ +void config_read_lighthouse(config_group* lh_config, BaseStationData* bsd, uint8_t idx) { + config_group *cg = lh_config + idx; + uint8_t found = 0; + for (int i = 0; i < NUM_LIGHTHOUSES; i++) + { + uint32_t tmpIdx = 0xffffffff; + cg = lh_config + idx; + + tmpIdx = config_read_uint32(cg, "index", 0xffffffff); + + if (tmpIdx == idx && i == idx) // assumes that lighthouses are stored in the config in order. + { + found = 1; + break; + } + } + + assert(found); // throw an assertion if we didn't find it... Is this good? not necessarily? + if (!found) + { + return; + } + + + FLT defaults[7] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + + bsd->BaseStationID = config_read_uint32(cg, "id", 0); + config_read_float_array(cg, "pose", &bsd->Pose.Pos[0], defaults, 7); + config_read_float_array(cg, "fcalphase", bsd->fcalphase, defaults, 2); + config_read_float_array(cg, "fcaltilt", bsd->fcaltilt, defaults, 2); + config_read_float_array(cg, "fcalcurve", bsd->fcalcurve, defaults, 2); + config_read_float_array(cg, "fcalgibpha", bsd->fcalgibpha, defaults, 2); + config_read_float_array(cg, "fcalgibmag", bsd->fcalgibmag, defaults, 2); +} + + void config_set_lighthouse(config_group* lh_config, BaseStationData* bsd, uint8_t idx) { config_group *cg = lh_config+idx; config_set_uint32(cg,"index", idx); @@ -143,18 +179,28 @@ FLT config_read_float(config_group *cg, const char *tag, const FLT def) { return config_set_float(cg, tag, def); } -uint16_t config_read_float_array(config_group *cg, const char *tag, const FLT** values, const FLT* def, uint8_t count) { +// TODO: Do something better than this: +#define CFG_MIN(x,y) ((x) < (y)? (x): (y)) + + +uint16_t config_read_float_array(config_group *cg, const char *tag, FLT* values, const FLT* def, uint8_t count) { config_entry *cv = find_config_entry(cg, tag); if (cv != NULL) { - *values = (FLT*)cv->data; + for (int i=0; i < CFG_MIN(count, cv->elements); i++) + { + values[i] = ((double*)cv->data)[i]; + } return cv->elements; } if (def == NULL) return 0; config_set_float_a(cg, tag, def, count); - *values = def; + for (int i = 0; i < count; i++) + { + values[i] = def[i]; + } return count; } diff --git a/src/survive_config.h b/src/survive_config.h index 83db624..e2686e9 100644 --- a/src/survive_config.h +++ b/src/survive_config.h @@ -43,6 +43,7 @@ void destroy_config_group(config_group* cg); //void config_write_lighthouse(struct BaseStationData* bsd, uint8_t length); void config_set_lighthouse(config_group* lh_config, BaseStationData* bsd, uint8_t idx); +void config_read_lighthouse(config_group* lh_config, BaseStationData* bsd, uint8_t idx); void config_read(SurviveContext* sctx, const char* path); void config_save(SurviveContext* sctx, const char* path); @@ -52,7 +53,7 @@ const uint32_t config_set_uint32(config_group *cg, const char *tag, const uint32 const char* config_set_str(config_group *cg, const char *tag, const char* value); FLT config_read_float(config_group *cg, const char *tag, const FLT def); -uint16_t config_read_float_array(config_group *cg, const char *tag, const FLT** values, const FLT* def, uint8_t count); +uint16_t config_read_float_array(config_group *cg, const char *tag, FLT* values, const FLT* def, uint8_t count); 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); -- cgit v1.2.3 From 283a395d8d63e4532173f9497f2ea8cd154a4758 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Fri, 22 Dec 2017 23:06:26 -0700 Subject: Configuration Changes Added some config options. Changed a few defaults. Better output from PoserTurveyTori. Hopefully, this will make it easier for someone new to get up and running more easily. --- src/poser_turveytori.c | 45 ++++++++++++++++++++++++++++++++++----------- src/survive.c | 6 +++++- src/survive_cal.c | 10 +++++++--- src/survive_config.c | 2 +- 4 files changed, 47 insertions(+), 16 deletions(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index d0b9ab8..ae4592d 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -15,6 +15,8 @@ #endif +static int ttDebug = 0; + #define PointToFlts(x) ((FLT*)(x)) typedef struct @@ -412,7 +414,7 @@ FLT getPointFitness(Point pointIn, PointsAndAngle *pna, size_t pnaCount, int deu } fitnesses[i] = FLT_FABS(fitness); - if (deubgPrint) + if (0) { printf(" [%d, %d](%f)\n", pna[i].ai, pna[i].bi, fitness); } @@ -585,7 +587,7 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, break; } } - printf(" i=%3d ", i); + if (ttDebug) printf(" i=%3d ", i); return lastPoint; } @@ -910,7 +912,7 @@ static void WhereIsTheTrackedObjectAxisAngle(FLT *posOut, FLT *rotation, Point l rotatearoundaxis(posOut, posOut, rotation, rotation[3]); - printf("{% 04.4f, % 04.4f, % 04.4f} ", posOut[0], posOut[1], posOut[2]); + if (ttDebug) printf("{% 04.4f, % 04.4f, % 04.4f} ", posOut[0], posOut[1], posOut[2]); } static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj) @@ -1011,7 +1013,7 @@ static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *ini break; } } - printf(" Ri=%d ", i); + if (ttDebug) printf(" Ri=%d ", i); } static void WhereIsTheTrackedObjectQuaternion(FLT *rotation, Point lhPoint) { @@ -1020,7 +1022,7 @@ static void WhereIsTheTrackedObjectQuaternion(FLT *rotation, Point lhPoint) //rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]); quatrotatevector(objPoint, rotation, objPoint); - printf("(%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]); + if (ttDebug) printf("(%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]); } @@ -1106,7 +1108,7 @@ static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *in //printf("+ %8.8f, (%8.8f, %8.8f, %8.8f) %f\n", newMatchFitness, point4[0], point4[1], point4[2], point4[3]); //#endif g *= 1.02; - printf("+"); + if (ttDebug) printf("+"); WhereIsTheTrackedObjectQuaternion(rotOut, lhPoint); } else @@ -1115,12 +1117,12 @@ static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *in //printf("- , %f\n", point4[3]); //#endif g *= 0.7; - printf("-"); + if (ttDebug) printf("-"); } } - printf("Ri=%3d Fitness=%3f ", i, lastMatchFitness); + if (ttDebug) printf("Ri=%3d Fitness=%3f ", i, lastMatchFitness); } @@ -1280,7 +1282,7 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob FLT fitGd = getPointFitness(refinedEstimateGd, pna, pnaCount, 0); FLT distance = FLT_SQRT(SQUARED(refinedEstimateGd.x) + SQUARED(refinedEstimateGd.y) + SQUARED(refinedEstimateGd.z)); - printf(" la(% 04.4f) SnsrCnt(%2d) LhPos:(% 04.4f, % 04.4f, % 04.4f) Dist: % 08.8f ", largestAngle, (int)obj->numSensors, refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance); + if (ttDebug) printf(" la(% 04.4f) SnsrCnt(%2d) LhPos:(% 04.4f, % 04.4f, % 04.4f) Dist: % 08.8f ", largestAngle, (int)obj->numSensors, refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance); //printf("Distance is %f, Fitness is %f\n", distance, fitGd); FLT rot[4]; // this is axis/ angle rotation, not a quaternion! @@ -1405,7 +1407,16 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob so->FromLHPose[lh].Rot[2] = so->OutPose.Rot[2]; so->FromLHPose[lh].Rot[3] = so->OutPose.Rot[3]; - printf(" <% 04.4f, % 04.4f, % 04.4f > ", wcPos[0], wcPos[1], wcPos[2]); + if (ttDebug) printf(" <% 04.4f, % 04.4f, % 04.4f > ", wcPos[0], wcPos[1], wcPos[2]); + + posOut[0] = wcPos[0]; + posOut[1] = wcPos[1]; + posOut[2] = wcPos[2]; + + quatOut[0] = so->OutPose.Rot[0]; + quatOut[1] = so->OutPose.Rot[1]; + quatOut[2] = so->OutPose.Rot[2]; + quatOut[3] = so->OutPose.Rot[3]; if (logFile) { @@ -1418,6 +1429,7 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob toriData->lastLhPos[lh].y = refinedEstimateGd.y; toriData->lastLhPos[lh].z = refinedEstimateGd.z; + return refinedEstimateGd; } @@ -1528,7 +1540,10 @@ static void QuickPose(SurviveObject *so, int lh) SolveForLighthouse(pos, quat, to, so, 0, lh, 0); - printf("!\n"); + + printf("P&O: [% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]); + + if (ttDebug) printf("!\n"); } @@ -1547,6 +1562,14 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) SurviveContext * ctx = so->ctx; ToriData * td = so->PoserData; + static int firstRun = 1; + + if (firstRun) + { + ttDebug = config_read_uint32(ctx->global_config_values, "TurveyToriDebug", 0); + + firstRun = 0; + } if (!td) { diff --git a/src/survive.c b/src/survive.c index 057462f..97c839b 100755 --- a/src/survive.c +++ b/src/survive.c @@ -96,7 +96,8 @@ SurviveContext * survive_init( int headless ) } i = 0; - const char * PreferredPoser = config_read_str( ctx->global_config_values, "DefaultPoser", "PoserDummy" ); + //const char * PreferredPoser = config_read_str(ctx->global_config_values, "DefaultPoser", "PoserDummy"); + const char * PreferredPoser = config_read_str(ctx->global_config_values, "DefaultPoser", "PoserTurveyTori"); PoserCB PreferredPoserCB = 0; const char * FirstPoser = 0; printf( "Available posers:\n" ); @@ -119,6 +120,9 @@ SurviveContext * survive_init( int headless ) ctx->objs[i]->PoserFn = PreferredPoserCB; } + // saving the config extra to make sure that the user has a config file they can change. + config_save(ctx, "config.json"); + return ctx; } diff --git a/src/survive_cal.c b/src/survive_cal.c index 0dbadd3..87d8c0b 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -124,8 +124,11 @@ void survive_cal_install( struct SurviveContext * ctx ) cd->numPoseObjects = 0; - const char * RequiredTrackersForCal = config_read_str( ctx->global_config_values, "RequiredTrackersForCal", "HMD,WM0,WM1" ); - const uint32_t AllowAllTrackersForCal = config_read_uint32( ctx->global_config_values, "AllowAllTrackersForCal", 0 ); + // setting the required trackers for calibration to be permissive to make it easier for a newbie to start-- + // basically, libsurvive will detect whatever they have plugged in and start using that. +// const char * RequiredTrackersForCal = config_read_str(ctx->global_config_values, "RequiredTrackersForCal", "HMD,WM0,WM1"); + const char * RequiredTrackersForCal = config_read_str(ctx->global_config_values, "RequiredTrackersForCal", ""); + const uint32_t AllowAllTrackersForCal = config_read_uint32( ctx->global_config_values, "AllowAllTrackersForCal", 1 ); size_t requiredTrackersFound = 0; for (int j=0; j < ctx->objs_ct; j++) @@ -169,7 +172,8 @@ void survive_cal_install( struct SurviveContext * ctx ) } const char * DriverName; - const char * PreferredPoser = config_read_str( ctx->global_config_values, "ConfigPoser", "PoserCharlesSlow" ); +// const char * PreferredPoser = config_read_str(ctx->global_config_values, "ConfigPoser", "PoserCharlesSlow"); + const char * PreferredPoser = config_read_str(ctx->global_config_values, "ConfigPoser", "PoserTurveyTori"); PoserCB PreferredPoserCB = 0; const char * FirstPoser = 0; printf( "Available posers:\n" ); diff --git a/src/survive_config.c b/src/survive_config.c index a24364f..0961651 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -103,7 +103,7 @@ void config_read_lighthouse(config_group* lh_config, BaseStationData* bsd, uint8 } } - assert(found); // throw an assertion if we didn't find it... Is this good? not necessarily? +// assert(found); // throw an assertion if we didn't find it... Is this good? not necessarily? if (!found) { return; -- cgit v1.2.3 From 417ced84b51bfcc392f06a87a1328f679364fc38 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Sat, 23 Dec 2017 12:49:42 -0700 Subject: Make Config Display a little more intuitive Changed the position markers to indicate +x as the "flagged" direction. Since the lighthouses face +x (based on arbitrary decision months ago), this makes their display more logical. --- calibrate.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/calibrate.c b/calibrate.c index 88f8288..8fb0986 100644 --- a/calibrate.c +++ b/calibrate.c @@ -238,30 +238,30 @@ void DisplayPose(SurvivePose pose, size_t xResolution, size_t yResolution) yResolution -(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1])); } - // Small line to indicate (0,+y) + // Small line to indicate (+x,0) { FLT tmp1[3]; FLT tmp1out[3]; FLT tmp2[3]; FLT tmp2out[3]; - tmp1[1] = minRectSize + ((pose.Pos[2] * 40.0)); + tmp1[0] = minRectSize + ((pose.Pos[2] * 40.0)); tmp1[2] = 0; - tmp1[0] = tmp1[1] * 0.3; + tmp1[1] = tmp1[0] * 0.3; quatrotatevector(tmp1out, pose.Rot, tmp1); - tmp2[1] = minRectSize + ((pose.Pos[2] * 40.0)); + tmp2[0] = minRectSize + ((pose.Pos[2] * 40.0)); tmp2[2] = 0; - tmp2[0] = -(tmp1[1] * 0.3); + tmp2[1] = -(tmp1[0] * 0.3); quatrotatevector(tmp2out, pose.Rot, tmp2); CNFGTackSegment( (short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp1out[0]), - yResolution -(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]), + yResolution - (short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]), (short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp2out[0]), - yResolution -(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1])); + yResolution - (short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1])); } -- cgit v1.2.3 From ec564d70daa8c1a66018f9606b02b873ae792c84 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Thu, 28 Dec 2017 08:03:37 -0700 Subject: Start work on determining rotation using quaternions only Rotation was previously approximated using axis/angle This change starts down the path of using quaternions exclusively. This change appears to give at least as good as answers as the axis/angle model in basic cases (also only tested with 1 lighthouse), but it is currently much slower and runs in unpredictable time. --- calibrate.c | 66 +++++++++++++++++++--- redist/linmath.c | 36 ++++++++++++ redist/linmath.h | 2 + src/poser_turveytori.c | 150 ++++++++++++++++++++++++++++++++++++++++--------- 4 files changed, 218 insertions(+), 36 deletions(-) diff --git a/calibrate.c b/calibrate.c index 8fb0986..cfd3e17 100644 --- a/calibrate.c +++ b/calibrate.c @@ -207,9 +207,9 @@ void DisplayPose(SurvivePose pose, size_t xResolution, size_t yResolution) CNFGTackSegment( (short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp1out[0]), - yResolution-(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]), + (short)yResolution-(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]), (short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp2out[0]), - yResolution -(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1])); + (short)yResolution -(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1])); } // line for the (0,-y) to (0,+y)) @@ -233,11 +233,36 @@ void DisplayPose(SurvivePose pose, size_t xResolution, size_t yResolution) CNFGTackSegment( (short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp1out[0]), - yResolution -(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]), + (short)yResolution -(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]), (short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp2out[0]), - yResolution -(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1])); + (short)yResolution -(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1])); } + // Small line to indicate (0,+y) + { + FLT tmp1[3]; + FLT tmp1out[3]; + FLT tmp2[3]; + FLT tmp2out[3]; + + tmp1[1] = minRectSize + ((pose.Pos[2] * 40.0)*1.3); + tmp1[2] = 0; + tmp1[0] = ((pose.Pos[2] * 40.0)) * 0.3; + + quatrotatevector(tmp1out, pose.Rot, tmp1); + + tmp2[1] = minRectSize + ((pose.Pos[2] * 40.0)*0.7); + tmp2[2] = 0; + tmp2[0] = -(((pose.Pos[2] * 40.0)) * 0.3); + + quatrotatevector(tmp2out, pose.Rot, tmp2); + + CNFGTackSegment( + (short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp1out[0]), + (short)yResolution - (short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]), + (short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp2out[0]), + (short)yResolution - (short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1])); + } // Small line to indicate (+x,0) { FLT tmp1[3]; @@ -245,13 +270,38 @@ void DisplayPose(SurvivePose pose, size_t xResolution, size_t yResolution) FLT tmp2[3]; FLT tmp2out[3]; - tmp1[0] = minRectSize + ((pose.Pos[2] * 40.0)); + tmp1[0] = minRectSize + ((pose.Pos[2] * 40.0)*1.3); tmp1[2] = 0; tmp1[1] = tmp1[0] * 0.3; quatrotatevector(tmp1out, pose.Rot, tmp1); - tmp2[0] = minRectSize + ((pose.Pos[2] * 40.0)); + tmp2[0] = minRectSize + ((pose.Pos[2] * 40.0)*.7); + tmp2[2] = 0; + tmp2[1] = -(tmp1[0] * 0.3); + + quatrotatevector(tmp2out, pose.Rot, tmp2); + + CNFGTackSegment( + (short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp1out[0]), + (short)yResolution - (short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]), + (short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp2out[0]), + (short)yResolution - (short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1])); + } + // Small line to indicate (+x,0) + { + FLT tmp1[3]; + FLT tmp1out[3]; + FLT tmp2[3]; + FLT tmp2out[3]; + + tmp1[0] = minRectSize + ((pose.Pos[2] * 40.0)*.7); + tmp1[2] = 0; + tmp1[1] = tmp1[0] * 0.3; + + quatrotatevector(tmp1out, pose.Rot, tmp1); + + tmp2[0] = minRectSize + ((pose.Pos[2] * 40.0)*1.3); tmp2[2] = 0; tmp2[1] = -(tmp1[0] * 0.3); @@ -259,9 +309,9 @@ void DisplayPose(SurvivePose pose, size_t xResolution, size_t yResolution) CNFGTackSegment( (short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp1out[0]), - yResolution - (short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]), + (short)yResolution - (short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]), (short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp2out[0]), - yResolution - (short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1])); + (short)yResolution - (short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1])); } diff --git a/redist/linmath.c b/redist/linmath.c index 5d51708..1d70ee1 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -143,6 +143,42 @@ void angleaxisfrom2vect(FLT *angle, FLT *axis, FLT *src, FLT *dest) } +void axisanglefromquat(FLT *angle, FLT *axis, FLT *q) +{ + // this way might be fine, too. + //FLT dist = FLT_SQRT((q[1] * q[1]) + (q[2] * q[2]) + (q[3] * q[3])); + // + //*angle = 2 * FLT_ATAN2(dist, q[0]); + + //axis[0] = q[1] / dist; + //axis[1] = q[2] / dist; + //axis[2] = q[3] / dist; + + + // Good mathematical foundation for this algorithm found here: + // http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle/index.htm + + FLT tmp[4] = { q[0], q[1], q[2], q[3] }; + + quatnormalize(tmp, q); + + if (FLT_FABS(q[0] - 1) < FLT_EPSILON) + { + // we have a degenerate case where we're rotating approx. 0 degrees + *angle = 0; + axis[0] = 1; + axis[1] = 0; + axis[2] = 0; + return; + } + + axis[0] = tmp[1] / sqrt(1 - (tmp[0] * tmp[0])); + axis[1] = tmp[2] / sqrt(1 - (tmp[0] * tmp[0])); + axis[2] = tmp[3] / sqrt(1 - (tmp[0] * tmp[0])); + + *angle = 2 * FLT_ACOS(tmp[0]); +} + /////////////////////////////////////QUATERNIONS////////////////////////////////////////// //Originally from Mercury (Copyright (C) 2009 by Joshua Allen, Charles Lohr, Adam Lowman) //Under the mit/X11 license. diff --git a/redist/linmath.h b/redist/linmath.h index 8d6cf05..1876b34 100644 --- a/redist/linmath.h +++ b/redist/linmath.h @@ -72,6 +72,8 @@ FLT anglebetween3d( FLT * a, FLT * b ); void rotatearoundaxis(FLT *outvec3, FLT *invec3, FLT *axis, FLT angle); void angleaxisfrom2vect(FLT *angle, FLT *axis, FLT *src, FLT *dest); +void axisanglefromquat(FLT *angle, FLT *axis, FLT *quat); + //Quaternion things... diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index ae4592d..fed5762 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -87,7 +87,8 @@ typedef struct int lastAxis[NUM_LIGHTHOUSES]; Point lastLhPos[NUM_LIGHTHOUSES]; - FLT lastLhRotAxisAngle[NUM_LIGHTHOUSES][4]; +// FLT lastLhRotAxisAngle[NUM_LIGHTHOUSES][4]; + FLT lastLhRotQuat[NUM_LIGHTHOUSES][4]; } ToriData; @@ -774,6 +775,21 @@ FLT RotationEstimateFitnessAxisAngleOriginal(Point lhPoint, FLT *quaternion, Tra // just an x or y axis to make our estimate better. TODO: bring that data to this fn. FLT RotationEstimateFitnessQuaternion(Point lhPoint, FLT *quaternion, TrackedObject *obj) { + +// TODO: ************************************************************************************************** THIS LIES!!!! NEED TO DO THIS IN QUATERNIONS!!!!!!!!!!!!!!!!! + { + FLT axisAngle[4]; + + axisanglefromquat(&(axisAngle[3]), axisAngle, quaternion); + + FLT throwaway = RotationEstimateFitnessAxisAngle(lhPoint, axisAngle, obj); + + int a = throwaway; + return throwaway; + } + + + FLT fitness = 0; for (size_t i = 0; i < obj->numSensors; i++) { @@ -1015,23 +1031,61 @@ static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *ini } if (ttDebug) printf(" Ri=%d ", i); } -static void WhereIsTheTrackedObjectQuaternion(FLT *rotation, Point lhPoint) +//static void WhereIsTheTrackedObjectQuaternion(FLT *rotation, Point lhPoint) +//{ +// FLT reverseRotation[4] = { rotation[0], rotation[1], rotation[2], -rotation[3] }; +// FLT objPoint[3] = { lhPoint.x, lhPoint.y, lhPoint.z }; +// +// //rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]); +// quatrotatevector(objPoint, rotation, objPoint); +// if (ttDebug) printf("(%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]); +//} +static void WhereIsTheTrackedObjectQuaternion(FLT *posOut, FLT *rotation, Point lhPoint) { - FLT reverseRotation[4] = {rotation[0], rotation[1], rotation[2], -rotation[3]}; - FLT objPoint[3] = {lhPoint.x, lhPoint.y, lhPoint.z}; - + posOut[0] = -lhPoint.x; + posOut[1] = -lhPoint.y; + posOut[2] = -lhPoint.z; + + FLT inverseRotation[4]; + + quatgetreciprocal(inverseRotation, rotation); + + FLT objPoint[3] = { lhPoint.x, lhPoint.y, lhPoint.z }; + //rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]); - quatrotatevector(objPoint, rotation, objPoint); - if (ttDebug) printf("(%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]); + quatrotatevector(posOut, inverseRotation, posOut); +// if (ttDebug) printf("(%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]); } +//static void WhereIsTheTrackedObjectAxisAngle(FLT *posOut, FLT *rotation, Point lhPoint) +//{ +// posOut[0] = -lhPoint.x; +// posOut[1] = -lhPoint.y; +// posOut[2] = -lhPoint.z; +// +// rotatearoundaxis(posOut, posOut, rotation, rotation[3]); +// +// if (ttDebug) printf("{% 04.4f, % 04.4f, % 04.4f} ", posOut[0], posOut[1], posOut[2]); +//} static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj) { int i = 0; + FLT lastMatchFitness = RotationEstimateFitnessQuaternion(lhPoint, initialEstimate, obj); + //{ + // FLT axisAngle[4]; + + // axisanglefromquat(&(axisAngle[3]), axisAngle, initialEstimate); + + // FLT throwaway = RotationEstimateFitnessAxisAngle(lhPoint, axisAngle, obj); + + // int a = throwaway; + //} + + quatcopy(rotOut, initialEstimate); // The values below are somewhat magic, and definitely tunable @@ -1108,8 +1162,8 @@ static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *in //printf("+ %8.8f, (%8.8f, %8.8f, %8.8f) %f\n", newMatchFitness, point4[0], point4[1], point4[2], point4[3]); //#endif g *= 1.02; - if (ttDebug) printf("+"); - WhereIsTheTrackedObjectQuaternion(rotOut, lhPoint); + printf("+"); + //WhereIsTheTrackedObjectQuaternion(rotOut, lhPoint); } else { @@ -1117,12 +1171,12 @@ static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *in //printf("- , %f\n", point4[3]); //#endif g *= 0.7; - if (ttDebug) printf("-"); + printf("-"); } } - if (ttDebug) printf("Ri=%3d Fitness=%3f ", i, lastMatchFitness); + printf("Ri=%3d Fitness=%3f ", i, lastMatchFitness); } @@ -1133,7 +1187,7 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) // This should have the lighthouse directly facing the tracked object. Point trackedObjRelativeToLh = { .x = -lh.x,.y = -lh.y,.z = -lh.z }; FLT theta = atan2(-lh.x, -lh.y); - FLT zAxis[4] = { 0, 0, 1 , theta-LINMATHPI/2}; + FLT zAxis[4] = { 0, 0, 1 , theta - LINMATHPI / 2 }; FLT quat1[4]; quatfromaxisangle(quat1, zAxis, theta); @@ -1152,6 +1206,32 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) } +void SolveForRotationQuat(FLT rotOut[4], TrackedObject *obj, Point lh) +{ + + // Step 1, create initial quaternion for guess. + // This should have the lighthouse directly facing the tracked object. + Point trackedObjRelativeToLh = { .x = -lh.x,.y = -lh.y,.z = -lh.z }; + FLT theta = atan2(-lh.x, -lh.y); + FLT zAxis[4] = { 0, 0, 1 , theta - LINMATHPI / 2 }; + FLT quat1[4]; + quatfromaxisangle(quat1, zAxis, theta); + + //quatfrom2vectors(0,0) + // not correcting for phi, but that's less important. + + + // Step 2, optimize the axis/ angle to match the data. + //RefineRotationEstimateAxisAngle(rotOut, lh, zAxis, obj); + + + //// Step 2, optimize the quaternion to match the data. + RefineRotationEstimateQuaternion(rotOut, lh, quat1, obj); + + //WhereIsTheTrackedObjectQuaternion(rotOut, lh); + +} + static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *obj, SurviveObject *so, char doLogOutput,const int lh,const int setLhCalibration) { @@ -1286,17 +1366,25 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob //printf("Distance is %f, Fitness is %f\n", distance, fitGd); FLT rot[4]; // this is axis/ angle rotation, not a quaternion! + FLT rotQuat[4]; // this is a quaternion! // if we've already guessed at the rotation of the lighthouse, // then let's use that as a starting guess, because it's probably // going to make convergence happen much faster. - if (toriData->lastLhRotAxisAngle[lh][0] != 0) - { - rot[0] = toriData->lastLhRotAxisAngle[lh][0]; - rot[1] = toriData->lastLhRotAxisAngle[lh][1]; - rot[2] = toriData->lastLhRotAxisAngle[lh][2]; - rot[3] = toriData->lastLhRotAxisAngle[lh][3]; - } + //if (toriData->lastLhRotAxisAngle[lh][0] != 0) + //{ + // rot[0] = toriData->lastLhRotAxisAngle[lh][0]; + // rot[1] = toriData->lastLhRotAxisAngle[lh][1]; + // rot[2] = toriData->lastLhRotAxisAngle[lh][2]; + // rot[3] = toriData->lastLhRotAxisAngle[lh][3]; + //} + //if (toriData->lastLhRotQuat[lh][0] != 0) + //{ + // rotQuat[0] = toriData->lastLhRotQuat[lh][0]; + // rotQuat[1] = toriData->lastLhRotQuat[lh][1]; + // rotQuat[2] = toriData->lastLhRotQuat[lh][2]; + // rotQuat[3] = toriData->lastLhRotQuat[lh][3]; + //} // Given the relative position of the lighthouse // to the tracked object, in the tracked object's coordinate @@ -1304,22 +1392,28 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob // tracked object's coordinate system. // TODO: I believe this could be radically improved // using an SVD. + SolveForRotationQuat(rotQuat, obj, refinedEstimateGd); SolveForRotation(rot, obj, refinedEstimateGd); FLT objPos[3]; + FLT objPos2[3]; - { - toriData->lastLhRotAxisAngle[lh][0] = rot[0]; - toriData->lastLhRotAxisAngle[lh][1] = rot[1]; - toriData->lastLhRotAxisAngle[lh][2] = rot[2]; - toriData->lastLhRotAxisAngle[lh][3] = rot[3]; - } + //{ + // toriData->lastLhRotQuat[lh][0] = rotQuat[0]; + // toriData->lastLhRotQuat[lh][1] = rotQuat[1]; + // toriData->lastLhRotQuat[lh][2] = rotQuat[2]; + // toriData->lastLhRotQuat[lh][3] = rotQuat[3]; + //} + + WhereIsTheTrackedObjectAxisAngle(objPos2, rot, refinedEstimateGd); + WhereIsTheTrackedObjectQuaternion(objPos, rotQuat, refinedEstimateGd); - WhereIsTheTrackedObjectAxisAngle(objPos, rot, refinedEstimateGd); + FLT rotQuat2[4]; + FLT rot2[4]; - FLT rotQuat[4]; + quatfromaxisangle(rotQuat2, rot, rot[3]); + axisanglefromquat(&(rot2[3]), rot2, rotQuat); - quatfromaxisangle(rotQuat, rot, rot[3]); //{ //FLT tmpPos[3] = {refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z}; -- cgit v1.2.3 From 6dea9ec51d21c9f58a6c82837b6dea0e67c20207 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Thu, 28 Dec 2017 08:25:02 -0700 Subject: MUCH better tracking Works with 2 lighthouses. Tracking from both lighthouses agree *much* better than before Inverting the tracker no longer screws up tracking Still much work to do to remove all axis angle and speed up/ make predictable the algorithm to estimate the rotation of the LH relative to the tracked object. --- src/poser_turveytori.c | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index fed5762..bd165ec 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1161,8 +1161,8 @@ static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *in //#ifdef TORI_DEBUG //printf("+ %8.8f, (%8.8f, %8.8f, %8.8f) %f\n", newMatchFitness, point4[0], point4[1], point4[2], point4[3]); //#endif - g *= 1.02; - printf("+"); + g *= 1.04; + //printf("+"); //WhereIsTheTrackedObjectQuaternion(rotOut, lhPoint); } else @@ -1171,7 +1171,8 @@ static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *in //printf("- , %f\n", point4[3]); //#endif g *= 0.7; - printf("-"); + //printf("-"); + //printf("%3f", lastMatchFitness); } @@ -1404,8 +1405,6 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob // toriData->lastLhRotQuat[lh][3] = rotQuat[3]; //} - WhereIsTheTrackedObjectAxisAngle(objPos2, rot, refinedEstimateGd); - WhereIsTheTrackedObjectQuaternion(objPos, rotQuat, refinedEstimateGd); FLT rotQuat2[4]; @@ -1415,6 +1414,10 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob axisanglefromquat(&(rot2[3]), rot2, rotQuat); +// WhereIsTheTrackedObjectAxisAngle(objPos, rot, refinedEstimateGd); // this is the original axis angle one + WhereIsTheTrackedObjectAxisAngle(objPos, rot2, refinedEstimateGd); // this one is axis angle, but using data derived by quaternions. + // WhereIsTheTrackedObjectQuaternion(objPos, rotQuat, refinedEstimateGd); <--------------This is hte one we need to use, might need to be fixed. + //{ //FLT tmpPos[3] = {refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z}; -- cgit v1.2.3 From 8eda10ed94b66a8bb388cc5710dc6b45b1901993 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Thu, 28 Dec 2017 08:56:43 -0700 Subject: Use accelerometer to determine "up" The accelerometer will be used to determine "up" instead of blindly using +z during calibration --- src/poser_turveytori.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index bd165ec..ac979c7 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1777,8 +1777,8 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] }; FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] }; - //quatrotatevector(norm, downQuat, norm); - //quatrotatevector(point, downQuat, point); + quatrotatevector(norm, downQuat, norm); + quatrotatevector(point, downQuat, point); //rotatearoundaxis(norm, norm, axis, angle); //rotatearoundaxis(point, point, axis, angle); @@ -1813,8 +1813,8 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] }; FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] }; - //quatrotatevector(norm, downQuat, norm); - //quatrotatevector(point, downQuat, point); + quatrotatevector(norm, downQuat, norm); + quatrotatevector(point, downQuat, point); //rotatearoundaxis(norm, norm, axis, angle); //rotatearoundaxis(point, point, axis, angle); @@ -1840,9 +1840,9 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) } - // This code block rotates the lighthouse fixes to accound for any time the tracked object + // This code block rotates the lighthouse fixes to account for any time the tracked object // is oriented other than +z = up - // This REALLY DOESN'T WORK!!! + //This REALLY DOESN'T WORK!!! //{ // for (int lh = 0; lh < 2; lh++) // { -- cgit v1.2.3 From 5272a639ab24c309ba695946098a0ce66b4078ad Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Mon, 1 Jan 2018 23:10:54 -0700 Subject: Start to support buttons Start the infrastructure for reading buttons from the tracked devices. Currently, only supporting wired controller and wired tracker. Data is printed only so far, not passed up the stack programmatically. --- src/survive_vive.c | 98 ++++++++++++++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 95 insertions(+), 3 deletions(-) diff --git a/src/survive_vive.c b/src/survive_vive.c index 9a3cb03..ef150f6 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -51,6 +51,11 @@ const short vidpids[] = { 0x28de, 0x2000, 1, //Valve HMD lighthouse(B) (only used on HIDAPI, for lightcap) 0x28de, 0x2022, 1, //HTC Tracker (only used on HIDAPI, for lightcap) 0x28de, 0x2012, 1, //Valve Watchman, USB connected (only used on HIDAPI, for lightcap) + + 0x28de, 0x2000, 2, //Valve HMD lighthouse(B) (only used on HIDAPI, for lightcap) + 0x28de, 0x2022, 2, //HTC Tracker (only used on HIDAPI, for lightcap) + 0x28de, 0x2012, 2, //Valve Watchman, USB connected (only used on HIDAPI, for lightcap) + #endif }; //length MAX_USB_INTERFACES*2 @@ -65,6 +70,10 @@ const char * devnames[] = { "HMD Lightcap", "Tracker 0 Lightcap", "Wired Watchman 1 Lightcap", + + "HMD Buttons", + "Tracker 0 Buttons", + "Wired Watchman 1 Buttons", #endif }; //length MAX_USB_INTERFACES @@ -80,7 +89,12 @@ const char * devnames[] = { #define USB_DEV_HMD_IMU_LHB 6 #define USB_DEV_TRACKER0_LIGHTCAP 7 #define USB_DEV_W_WATCHMAN1_LIGHTCAP 8 -#define MAX_USB_DEVS 9 + +#define USB_DEV_HMD_BUTTONS 9 +#define USB_DEV_TRACKER0_BUTTONS 10 +#define USB_DEV_W_WATCHMAN1_BUTTONS 11 + +#define MAX_USB_DEVS 12 #else #define MAX_USB_DEVS 6 #endif @@ -94,7 +108,10 @@ const char * devnames[] = { #define USB_IF_LIGHTCAP 6 #define USB_IF_TRACKER0_LIGHTCAP 7 #define USB_IF_W_WATCHMAN1_LIGHTCAP 8 -#define MAX_INTERFACES 9 +#define USB_IF_HMD_BUTTONS 9 +#define USB_IF_TRACKER0_BUTTONS 10 +#define USB_IF_W_WATCHMAN1_BUTTONS 11 +#define MAX_INTERFACES 12 typedef struct SurviveUSBInterface SurviveUSBInterface; typedef struct SurviveViveData SurviveViveData; @@ -487,7 +504,13 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject * // This is a HACK! But it works. Need to investigate further sv->uiface[USB_DEV_TRACKER0_LIGHTCAP].actual_len = 64; if( sv->udev[USB_DEV_TRACKER0_LIGHTCAP] && AttachInterface( sv, tr0, USB_IF_TRACKER0_LIGHTCAP, sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0x82, survive_data_cb, "Tracker 1 Lightcap")) { return -13; } + if( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP] && AttachInterface( sv, ww0, USB_IF_W_WATCHMAN1_LIGHTCAP, sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0x82, survive_data_cb, "Wired Watchman 1 Lightcap")) { return -13; } + + + if (sv->udev[USB_DEV_TRACKER0_BUTTONS] && AttachInterface(sv, tr0, USB_IF_TRACKER0_BUTTONS, sv->udev[USB_DEV_TRACKER0_BUTTONS], 0x83, survive_data_cb, "Tracker 1 Buttons")) { return -13; } + if (sv->udev[USB_DEV_W_WATCHMAN1_BUTTONS] && AttachInterface(sv, ww0, USB_IF_W_WATCHMAN1_BUTTONS, sv->udev[USB_DEV_W_WATCHMAN1_BUTTONS], 0x83, survive_data_cb, "Wired Watchman 1 BUTTONS")) { return -13; } + #else if( sv->udev[USB_DEV_HMD_IMU_LH] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_HMD_IMU_LH], 0x82, survive_data_cb, "Lightcap")) { return -12; } if( sv->udev[USB_DEV_TRACKER0] && AttachInterface( sv, ww0, USB_IF_TRACKER0_LIGHTCAP, sv->udev[USB_DEV_TRACKER0], 0x82, survive_data_cb, "Tracker 0 Lightcap")) { return -13; } @@ -1192,7 +1215,10 @@ void survive_data_cb( SurviveUSBInterface * si ) ctx->imuproc( obj, 3, agm, timecode, code ); } } - + if (id != 32) + { + int a=0; // set breakpoint here + } //DONE OK. break; } @@ -1250,7 +1276,73 @@ void survive_data_cb( SurviveUSBInterface * si ) handle_lightcap( obj, &le ); } break; + + if (id != 33) + { + int a = 0; // breakpoint here + } + } + case USB_IF_TRACKER0_BUTTONS: + case USB_IF_W_WATCHMAN1_BUTTONS: + { + if (1 == id) + { + //0x00 uint8 1 reportID HID report identifier(= 1) + //0x02 uint16 2 reportType(? ) 0x0B04: Ping(every second) / 0x3C01 : User input + //0x04 uint32 4 reportCount Counter that increases with every report + //0x08 uint32 4 pressedButtons Bit field, see below for individual buttons + //0x0C uint16 2 triggerOrBattery Analog trigger value(user input) / Battery voltage ? (ping) + //0x0E uint8 1 batteryCharge Bit 7 : Charging / Bit 6..0 : Battery charge in percent + //0x10 uint32 4 hardwareID Hardware ID(user input) / 0x00000000 (ping) + //0x14 int16 2 touchpadHorizontal Horizontal thumb position(Left : -32768 / Right : 32767) + //0x16 int16 2 touchpadVertical Vertical thumb position(Bottom : -32768 / Top : 32767) + //0x18 ? 2 ? unknown + //0x1A uint16 2 triggerHighRes Analog trigger value with higher resolution + //0x1C ? 24 ? unknown + //0x34 uint16 2 triggerRawMaybe Analog trigger value, maybe raw sensor data + //0x36 ? 8 ? unknown + //0x3E uint8 1 someBitFieldMaybe 0x00 : ping / 0x64 : user input + //0x3F ? 1 ? unknown + + typedef struct + { + //uint8_t reportId; + uint16_t reportType; + uint32_t reportCount; + uint32_t pressedButtons; + uint16_t triggerOrBattery; + uint16_t batteryCharge; + uint32_t hardwareId; + int16_t touchpadHorizontal; + int16_t touchpadVertical; + uint16_t unknown1; + uint16_t triggerHighRes; + uint8_t unknown2; + uint8_t unknown3; + uint8_t unknown4; + uint16_t triggerRaw; + uint8_t unknown5; + uint8_t unknown6; // maybe some bitfield? + uint8_t unknown7; + } usb_buttons_raw; + + usb_buttons_raw *raw = (usb_buttons_raw*) readdata; + if (raw->reportType == 0x100) + { + printf("Buttons: %8.8x\n", raw->pressedButtons); + } + int a = 0; + } + else + { + int a = 0;// breakpoint here + } } + default: + { + int a = 0; // breakpoint here + } + } } -- cgit v1.2.3 From 04e44b9f1c1b65198ea1ac883dcd9f153933413d Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Tue, 2 Jan 2018 21:37:57 -0700 Subject: Capturing Button Inputs Currently only working on Windows over USB interface Inputs are only printed out, still need to propagate them up the stack. Buttons are placed in a queue, and "processed" on a different thread to avoid starvation. --- include/libsurvive/survive.h | 40 +++++++- src/survive.c | 55 ++++++++++ src/survive_vive.c | 233 ++++++++++++++++++++++++++++++++++++++----- 3 files changed, 301 insertions(+), 27 deletions(-) diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index c4b2abe..f3a17c8 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -4,6 +4,7 @@ #include #include "survive_types.h" #include "poser.h" +#include "os_generic.h" #ifdef __cplusplus extern "C" { @@ -18,7 +19,7 @@ struct SurviveObject char codename[4]; //3 letters, null-terminated. Currently HMD, WM0, WM1. char drivername[4]; //3 letters for driver. Currently "HTC" - int16_t buttonmask; + int32_t buttonmask; int16_t axis1; int16_t axis2; @@ -90,6 +91,37 @@ struct BaseStationData struct config_group; +#define BUTTON_QUEUE_MAX_LEN 32 + +#define BUTTON_EVENT_BUTTON_NONE 0 +#define BUTTON_EVENT_BUTTON_DOWN 1 +#define BUTTON_EVENT_BUTTON_UP 2 +#define BUTTON_EVENT_AXIS_CHANGED 3 + +// note: buttonId and axisId are 1-indexed values. +// a value of 0 for an id means that no data is present in that value +// additionally, when x and y values are both present in axis data, +// axis1 will be x, axis2 will be y. +typedef struct +{ + uint8_t isPopulated; //probably can remove this given the semaphore in the parent struct. helps with debugging + uint8_t eventType; + uint8_t buttonId; + uint8_t axis1Id; + uint16_t axis1Val; + uint8_t axis2Id; + uint16_t axis2Val; + SurviveObject *so; +} ButtonQueueEntry; + +typedef struct +{ + uint8_t nextReadIndex; //init to 0 + uint8_t nextWriteIndex; // init to 0 + og_sema_t buttonservicesem; + ButtonQueueEntry entry[BUTTON_QUEUE_MAX_LEN]; +} ButtonQueue; + struct SurviveContext { text_feedback_func faultfunction; @@ -114,6 +146,12 @@ struct SurviveContext DeviceDriverCb * drivercloses; DeviceDriverMagicCb * drivermagics; int driver_ct; + + uint8_t isClosing; // flag to indicate if threads should terminate themselves + + og_thread_t buttonservicethread; + ButtonQueue buttonQueue; + }; SurviveContext * survive_init( int headless ); diff --git a/src/survive.c b/src/survive.c index 97c839b..99b6648 100755 --- a/src/survive.c +++ b/src/survive.c @@ -40,6 +40,45 @@ static void survivenote( struct SurviveContext * ctx, const char * fault ) fprintf( stderr, "Info: %s\n", fault ); } +static void button_servicer(void * context) +{ + SurviveContext *ctx = (SurviveContext*)context; + + while (1) + { + OGLockSema(ctx->buttonQueue.buttonservicesem); + + if (ctx->isClosing) + { + // we're shutting down. Close. + return; + } + + ButtonQueueEntry *entry = &(ctx->buttonQueue.entry[ctx->buttonQueue.nextReadIndex]); + if (entry->isPopulated == 0) + { + // should never happen. indicates failure of code pushing stuff onto + // the buttonQueue + // if it does happen, it will kill all future button input + printf("ERROR: Unpopulated ButtonQueueEntry!"); + return; + } + + printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n", + entry->eventType, + entry->buttonId, + entry->axis1Id, + entry->axis1Val, + entry->axis2Id, + entry->axis2Val); + + ctx->buttonQueue.nextReadIndex++; + if (ctx->buttonQueue.nextReadIndex >= BUTTON_QUEUE_MAX_LEN) + { + ctx->buttonQueue.nextReadIndex = 0; + } + }; +} SurviveContext * survive_init( int headless ) { @@ -66,6 +105,8 @@ SurviveContext * survive_init( int headless ) int i = 0; SurviveContext * ctx = calloc( 1, sizeof( SurviveContext ) ); + ctx->isClosing = 0; + ctx->global_config_values = malloc( sizeof(config_group) ); ctx->lh_config = malloc( sizeof(config_group) * NUM_LIGHTHOUSES); @@ -123,6 +164,14 @@ SurviveContext * survive_init( int headless ) // saving the config extra to make sure that the user has a config file they can change. config_save(ctx, "config.json"); + // initialize the button queue + memset(&(ctx->buttonQueue), 0, sizeof(ctx->buttonQueue)); + + ctx->buttonQueue.buttonservicesem = OGCreateSema(); + + // start the thread to process button data + ctx->buttonservicethread = OGCreateThread(button_servicer, ctx); + return ctx; } @@ -205,6 +254,12 @@ void survive_close( SurviveContext * ctx ) { const char * DriverName; int r = 0; + + ctx->isClosing = 1; + + // unlock/ post to button service semaphore so the thread can kill itself + OGUnlockSema(ctx->buttonQueue.buttonservicesem); + while( ( DriverName = GetDriverNameMatching( "DriverUnreg", r++ ) ) ) { DeviceDriver dd = GetDriver( DriverName ); diff --git a/src/survive_vive.c b/src/survive_vive.c index ef150f6..146ea01 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -865,6 +865,162 @@ void calibrate_gyro(SurviveObject* so, FLT* agm) { } } +typedef struct +{ + // could use a bitfield here, but since this data is short-lived, + // the space savings probably isn't worth the processing overhead. + uint8_t pressedButtonsValid; + uint8_t triggerOfBatteryValid; + uint8_t batteryChargeValid; + uint8_t hardwareIdValid; + uint8_t touchpadHorizontalValid; + uint8_t touchpadVerticalValid; + uint8_t triggerHighResValid; + + uint32_t pressedButtons; + uint16_t triggerOrBattery; + uint8_t batteryCharge; + uint32_t hardwareId; + uint16_t touchpadHorizontal; + uint16_t touchpadVertical; + uint16_t triggerHighRes; +} buttonEvent; + +void incrementAndPostButtonQueue(SurviveContext *ctx) +{ + ButtonQueueEntry *entry = &(ctx->buttonQueue.entry[ctx->buttonQueue.nextWriteIndex]); + + if (OGGetSema(ctx->buttonQueue.buttonservicesem) >= BUTTON_QUEUE_MAX_LEN) + { + // There's not enough space to write this entry. Clear it out and move along + memset(entry, 0, sizeof(ButtonQueueEntry)); + return; + } + entry->isPopulated = 1; + ctx->buttonQueue.nextWriteIndex++; + // if we've exceeded the size of the buffer, loop around to the beginning. + if (ctx->buttonQueue.nextWriteIndex >= BUTTON_QUEUE_MAX_LEN) + { + ctx->buttonQueue.nextWriteIndex = 0; + } + OGUnlockSema(ctx->buttonQueue.buttonservicesem); + + // clear out any old data in the entry so we always start with a clean slate. + entry = &(ctx->buttonQueue.entry[ctx->buttonQueue.nextWriteIndex]); + memset(entry, 0, sizeof(ButtonQueueEntry)); +} + +// important! This must be the only place that we're posting to the buttonEntryQueue +// if that ever needs to be changed, you will have to add locking so that only one +// thread is posting at a time. +void registerButtonEvent(SurviveObject *so, buttonEvent *event) +{ + ButtonQueueEntry *entry = &(so->ctx->buttonQueue.entry[so->ctx->buttonQueue.nextWriteIndex]); + + memset(entry, 0, sizeof(ButtonQueueEntry)); + + entry->so = so; + if (event->pressedButtonsValid) + { + //printf("trigger %8.8x\n", event->triggerHighRes); + for (int a=0; a < 16; a++) + { + if (((event->pressedButtons) & (1<buttonmask) & (1<pressedButtons & (1 << a)) + { + // it went down + entry->eventType = BUTTON_EVENT_BUTTON_DOWN; + } + else + { + // it went up + entry->eventType = BUTTON_EVENT_BUTTON_UP; + } + entry->buttonId = a; + incrementAndPostButtonQueue(so->ctx); + entry = &(so->ctx->buttonQueue.entry[so->ctx->buttonQueue.nextWriteIndex]); + } + } + // if the trigger button is depressed & it wasn't before + if ((((event->pressedButtons) & (0xff000000)) == 0xff000000) && + ((so->buttonmask) & (0xff000000)) != 0xff000000) + { + entry->eventType = BUTTON_EVENT_BUTTON_DOWN; + entry->buttonId = 24; + incrementAndPostButtonQueue(so->ctx); + entry = &(so->ctx->buttonQueue.entry[so->ctx->buttonQueue.nextWriteIndex]); + } + // if the trigger button isn't depressed but it was before + else if ((((event->pressedButtons) & (0xff000000)) != 0xff000000) && + ((so->buttonmask) & (0xff000000)) == 0xff000000) + { + entry->eventType = BUTTON_EVENT_BUTTON_UP; + entry->buttonId = 24; + incrementAndPostButtonQueue(so->ctx); + entry = &(so->ctx->buttonQueue.entry[so->ctx->buttonQueue.nextWriteIndex]); + } + } + if (event->triggerHighResValid) + { + if (so->axis1 != event->triggerHighRes) + { + entry->eventType = BUTTON_EVENT_AXIS_CHANGED; + entry->axis1Id = 1; + entry->axis1Val = event->triggerHighRes; + incrementAndPostButtonQueue(so->ctx); + entry = &(so->ctx->buttonQueue.entry[so->ctx->buttonQueue.nextWriteIndex]); + + } + } + if ((event->touchpadHorizontalValid) && (event->touchpadVerticalValid)) + { + if ((so->axis2 != event->touchpadHorizontal) || + (so->axis3 != event->touchpadVertical)) + { + entry->eventType = BUTTON_EVENT_AXIS_CHANGED; + entry->axis1Id = 2; + entry->axis1Val = event->touchpadHorizontal; + entry->axis2Id = 3; + entry->axis2Val = event->touchpadVertical; + incrementAndPostButtonQueue(so->ctx); + entry = &(so->ctx->buttonQueue.entry[so->ctx->buttonQueue.nextWriteIndex]); + + } + } + + if (event->pressedButtonsValid) + { + so->buttonmask = event->pressedButtons; + } + if (event->batteryChargeValid) + { + so->charge = event->batteryCharge; + } + if (event->touchpadHorizontalValid) + { + so->axis2 = event->touchpadHorizontal; + } + if (event->touchpadVerticalValid) + { + so->axis3 = event->touchpadVertical; + } + if (event->triggerHighResValid) + { + so->axis1 = event->triggerHighRes; + } +} + +uint8_t isPopulated; //probably can remove this given the semaphore in the parent struct. helps with debugging +uint8_t eventType; +uint8_t buttonId; +uint8_t axis1Id; +uint16_t axis1Val; +uint8_t axis2Id; +uint16_t axis2Val; +SurviveObject *so; + static void handle_watchman( SurviveObject * w, uint8_t * readdata ) { @@ -885,6 +1041,8 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) uint8_t qty = POP1; uint8_t time2 = POP1; uint8_t type = POP1; + + qty-=2; int propset = 0; int doimu = 0; @@ -899,6 +1057,8 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) { qty-=1; w->buttonmask = POP1; + + printf("buttonmask is %d\n", w->buttonmask); type &= ~0x01; } if( type & 0x04 ) @@ -1237,7 +1397,7 @@ void survive_data_cb( SurviveUSBInterface * si ) } else if( id == 38 ) { - w->ison = 0; + w->ison = 0; // turning off } else { @@ -1304,32 +1464,53 @@ void survive_data_cb( SurviveUSBInterface * si ) //0x3E uint8 1 someBitFieldMaybe 0x00 : ping / 0x64 : user input //0x3F ? 1 ? unknown - typedef struct - { - //uint8_t reportId; - uint16_t reportType; - uint32_t reportCount; - uint32_t pressedButtons; - uint16_t triggerOrBattery; - uint16_t batteryCharge; - uint32_t hardwareId; - int16_t touchpadHorizontal; - int16_t touchpadVertical; - uint16_t unknown1; - uint16_t triggerHighRes; - uint8_t unknown2; - uint8_t unknown3; - uint8_t unknown4; - uint16_t triggerRaw; - uint8_t unknown5; - uint8_t unknown6; // maybe some bitfield? - uint8_t unknown7; - } usb_buttons_raw; - - usb_buttons_raw *raw = (usb_buttons_raw*) readdata; - if (raw->reportType == 0x100) + //typedef struct + //{ + // //uint8_t reportId; + // uint16_t reportType; + // uint32_t reportCount; + // uint32_t pressedButtons; + // uint16_t triggerOrBattery; + // uint8_t batteryCharge; + // uint32_t hardwareId; + // int16_t touchpadHorizontal; + // int16_t touchpadVertical; + // uint16_t unknown1; + // uint16_t triggerHighRes; + // uint8_t unknown2; + // uint8_t unknown3; + // uint8_t unknown4; + // uint16_t triggerRaw; + // uint8_t unknown5; + // uint8_t unknown6; // maybe some bitfield? + // uint8_t unknown7; + //} usb_buttons_raw; + + //usb_buttons_raw *raw = (usb_buttons_raw*) readdata; + if (*((uint16_t*)(&(readdata[0x0]))) == 0x100) { - printf("Buttons: %8.8x\n", raw->pressedButtons); + buttonEvent bEvent; + memset(&bEvent, 0, sizeof(bEvent)); + + bEvent.pressedButtonsValid = 1; + bEvent.pressedButtons = *((uint32_t*)(&(readdata[0x07]))); + bEvent.triggerHighResValid = 1; + //bEvent.triggerHighRes = raw->triggerHighRes; + //bEvent.triggerHighRes = (raw->pressedButtons & 0xff000000) >> 24; // this seems to provide the same data at 2x the resolution as above + //bEvent.triggerHighRes = raw->triggerRaw; + + bEvent.triggerHighRes = *((uint16_t*)(&(readdata[0x19]))); + bEvent.touchpadHorizontalValid = 1; + //bEvent.touchpadHorizontal = raw->touchpadHorizontal; + bEvent.touchpadHorizontal = *((int16_t*)(&(readdata[0x13]))); + bEvent.touchpadVerticalValid = 1; + //bEvent.touchpadVertical = raw->touchpadVertical; + bEvent.touchpadVertical = *((int16_t*)(&(readdata[0x15]))); + + //printf("%4.4x\n", bEvent.triggerHighRes); + registerButtonEvent(obj, &bEvent); + + //printf("Buttons: %8.8x\n", raw->pressedButtons); } int a = 0; } -- cgit v1.2.3 From 7ef98a611c4e32dc89eba26581f7245797d0623a Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Tue, 2 Jan 2018 21:51:28 -0700 Subject: Add button input support for wireless --- src/survive_vive.c | 33 ++++++++++++++++++++++++++++----- 1 file changed, 28 insertions(+), 5 deletions(-) diff --git a/src/survive_vive.c b/src/survive_vive.c index 146ea01..048aad4 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -939,6 +939,14 @@ void registerButtonEvent(SurviveObject *so, buttonEvent *event) entry->eventType = BUTTON_EVENT_BUTTON_UP; } entry->buttonId = a; + if (entry->buttonId == 0) + { + // this fixes 2 issues. First, is the a button id of 0 indicates no button pressed. + // second is that the trigger shows up as button 0 coming from the wireless controller, + // but we infer it from the position on the wired controller. On the wired, we treat it + // as buttonId 24 (look further down in this function) + entry->buttonId = 24; + } incrementAndPostButtonQueue(so->ctx); entry = &(so->ctx->buttonQueue.entry[so->ctx->buttonQueue.nextWriteIndex]); } @@ -1049,32 +1057,47 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) if( (type & 0xf0) == 0xf0 ) { + buttonEvent bEvent; + memset(&bEvent, 0, sizeof(bEvent)); + propset |= 4; //printf( "%02x %02x %02x %02x\n", qty, type, time1, time2 ); type &= ~0x10; if( type & 0x01 ) { + qty-=1; - w->buttonmask = POP1; + bEvent.pressedButtonsValid = 1; + bEvent.pressedButtons = POP1; - printf("buttonmask is %d\n", w->buttonmask); + //printf("buttonmask is %d\n", w->buttonmask); type &= ~0x01; } if( type & 0x04 ) { qty-=1; - w->axis1 = ( POP1 ) * 128; + bEvent.triggerHighResValid = 1; + bEvent.triggerHighRes = ( POP1 ) * 128; type &= ~0x04; } if( type & 0x02 ) { qty-=4; - w->axis2 = POP2; - w->axis3 = POP2; + bEvent.touchpadHorizontalValid = 1; + bEvent.touchpadVerticalValid = 1; + + bEvent.touchpadHorizontal = POP2; + bEvent.touchpadVertical = POP2; type &= ~0x02; } + if (bEvent.pressedButtonsValid || bEvent.triggerHighResValid || bEvent.touchpadHorizontalValid) + { + registerButtonEvent(w, &bEvent); + } + + //XXX TODO: Is this correct? It looks SO WACKY type &= 0x7f; if( type == 0x68 ) doimu = 1; -- cgit v1.2.3 From ffb879d7a69c3c38937ff0159de0a5d1db99b713 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Tue, 2 Jan 2018 23:47:22 -0700 Subject: Haptic POC --Upon startup, Watchman1 (wireless only) will do a short haptic feedback/ vibration. --Also have some POC code that turns off the controller (currently disabled) --Thank you Nairol! --- src/survive_vive.c | 27 ++++++++++++++++++++------- 1 file changed, 20 insertions(+), 7 deletions(-) diff --git a/src/survive_vive.c b/src/survive_vive.c index 048aad4..e271e3e 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -619,18 +619,31 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo //#endif +//#if 0 + for (int i = 0; i < 0xff; i++) + { + //uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x7, 0 /*right*/, 0xFF /*period on*/, 0xFF/*period on*/, 0xFF/*period off*/, 0xFF/*period off*/, 0xFF /* repeat Count */, 0xFF /* repeat count */ }; + //uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x07, 0x00, 0xf4, 0x01, 0xb5, 0xa2, 0x01, 0x00 }; // data taken from Nairol's captures + uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x07, 0x00, 0xf4, 0x01, 0xb5, 0xa2, 0x01, 0x00 }; + //r = update_feature_report( sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) ); + 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)) printf("HAPTIC FAILED **************************\n"); // return 5; + OGUSleep(1000); + } +//#endif + #if 0 - for( int i = 0; i < 256; i++ ) + // working code to turn off a wireless controller: { - static uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0xff, 0, 0, 0, 0, 0, 0, 0 }; + uint8_t vive_controller_off[64] = { 0xff, 0x9f, 0x04, 'o', 'f', 'f', '!' }; //r = update_feature_report( sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) ); - r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) ); - SV_INFO( "UCR: %d", r ); - if( r != sizeof( vive_controller_haptic_pulse ) ) return 5; - OGUSleep( 1000 ); + r = update_feature_report(sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_off, sizeof(vive_controller_off)); + SV_INFO("UCR: %d", r); + if (r != sizeof(vive_controller_off)) printf("OFF FAILED **************************\n"); // return 5; + OGUSleep(1000); } #endif - //if (sv->udev[USB_DEV_TRACKER0]) //{ // static uint8_t vive_magic_power_on[64] = { 0x04, 0x78, 0x29, 0x38 }; -- cgit v1.2.3 From f183aa480c549695ac5b481fade04e62f71d1e0a Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Wed, 3 Jan 2018 18:58:43 -0700 Subject: Controller Buttons Fully Implemented Fully plumbed support for controller buttons Also, commented haptic call because it messed with the vive_magic calls, given where I had it. --- include/libsurvive/survive.h | 8 ++++---- include/libsurvive/survive_types.h | 8 +++++++- src/survive.c | 36 +++++++++++++++++++++++++++++------- src/survive_process.c | 11 +++++++++++ src/survive_vive.c | 28 ++++++++++++++-------------- 5 files changed, 65 insertions(+), 26 deletions(-) diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index f3a17c8..e4afadf 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -93,10 +93,7 @@ struct config_group; #define BUTTON_QUEUE_MAX_LEN 32 -#define BUTTON_EVENT_BUTTON_NONE 0 -#define BUTTON_EVENT_BUTTON_DOWN 1 -#define BUTTON_EVENT_BUTTON_UP 2 -#define BUTTON_EVENT_AXIS_CHANGED 3 + // note: buttonId and axisId are 1-indexed values. // a value of 0 for an id means that no data is present in that value @@ -129,6 +126,7 @@ struct SurviveContext light_process_func lightproc; imu_process_func imuproc; angle_process_func angleproc; + button_process_func buttonproc; struct config_group* global_config_values; struct config_group* lh_config; //lighthouse configs @@ -163,6 +161,7 @@ void survive_install_error_fn( SurviveContext * ctx, text_feedback_func fbp ); void survive_install_light_fn( SurviveContext * ctx, light_process_func fbp ); void survive_install_imu_fn( SurviveContext * ctx, imu_process_func fbp ); void survive_install_angle_fn( SurviveContext * ctx, angle_process_func fbp ); +void survive_install_button_fn( SurviveContext * ctx, button_process_func fbp ); void survive_close( SurviveContext * ctx ); int survive_poll( SurviveContext * ctx ); @@ -185,6 +184,7 @@ int survive_cal_get_status( struct SurviveContext * ctx, char * description, int void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length , uint32_t lh); void survive_default_imu_process( SurviveObject * so, int mode, FLT * accelgyro, uint32_t timecode, int id ); void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh ); +void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val); ////////////////////// Survive Drivers //////////////////////////// diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h index 224719e..fa3eb2f 100644 --- a/include/libsurvive/survive_types.h +++ b/include/libsurvive/survive_types.h @@ -27,6 +27,12 @@ typedef struct SurvivePose #define INTBUFFSIZE 64 #define SENSORS_PER_OBJECT 32 +// These are used for the eventType of button_process_func +#define BUTTON_EVENT_BUTTON_NONE 0 +#define BUTTON_EVENT_BUTTON_DOWN 1 +#define BUTTON_EVENT_BUTTON_UP 2 +#define BUTTON_EVENT_AXIS_CHANGED 3 + typedef struct SurviveObject SurviveObject; typedef struct SurviveContext SurviveContext; typedef struct BaseStationData BaseStationData; @@ -36,7 +42,7 @@ typedef void (*text_feedback_func)( SurviveContext * ctx, const char * fault ); typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lighthouse); typedef void (*imu_process_func)( SurviveObject * so, int mask, FLT * accelgyro, uint32_t timecode, int id ); typedef void (*angle_process_func)( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh); - +typedef void (*button_process_func)(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val); //Device drivers (prefix your drivers with "DriverReg") i.e. // REGISTER_LINKTIME( DriverRegHTCVive ); diff --git a/src/survive.c b/src/survive.c index 99b6648..d3c0918 100755 --- a/src/survive.c +++ b/src/survive.c @@ -64,13 +64,25 @@ static void button_servicer(void * context) return; } - printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n", - entry->eventType, - entry->buttonId, - entry->axis1Id, - entry->axis1Val, - entry->axis2Id, - entry->axis2Val); + //printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n", + // entry->eventType, + // entry->buttonId, + // entry->axis1Id, + // entry->axis1Val, + // entry->axis2Id, + // entry->axis2Val); + + button_process_func butt_func = ctx->buttonproc; + if (butt_func) + { + butt_func(entry->so, + entry->eventType, + entry->buttonId, + entry->axis1Id, + entry->axis1Val, + entry->axis2Id, + entry->axis2Val); + } ctx->buttonQueue.nextReadIndex++; if (ctx->buttonQueue.nextReadIndex >= BUTTON_QUEUE_MAX_LEN) @@ -171,6 +183,7 @@ SurviveContext * survive_init( int headless ) // start the thread to process button data ctx->buttonservicethread = OGCreateThread(button_servicer, ctx); + survive_install_button_fn(ctx, NULL); return ctx; } @@ -216,6 +229,15 @@ void survive_install_angle_fn( SurviveContext * ctx, angle_process_func fbp ) ctx->angleproc = survive_default_angle_process; } +void survive_install_button_fn(SurviveContext * ctx, button_process_func fbp) +{ + if (fbp) + ctx->buttonproc = fbp; + else + ctx->buttonproc = survive_default_button_process; + +} + int survive_add_object( SurviveContext * ctx, SurviveObject * obj ) { int oldct = ctx->objs_ct; diff --git a/src/survive_process.c b/src/survive_process.c index 3af2da9..eaed881 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -63,6 +63,17 @@ void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode } } +void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val) +{ + // do nothing. + printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n", + eventType, + buttonId, + axis1Id, + axis1Val, + axis2Id, + axis2Val); +} void survive_default_imu_process( SurviveObject * so, int mask, FLT * accelgyromag, uint32_t timecode, int id ) { diff --git a/src/survive_vive.c b/src/survive_vive.c index e271e3e..3df60ba 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -619,30 +619,30 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo //#endif -//#if 0 - for (int i = 0; i < 0xff; i++) +#if 0 + for (int i = 0; i < 0xf; i++) { //uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x7, 0 /*right*/, 0xFF /*period on*/, 0xFF/*period on*/, 0xFF/*period off*/, 0xFF/*period off*/, 0xFF /* repeat Count */, 0xFF /* repeat count */ }; //uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x07, 0x00, 0xf4, 0x01, 0xb5, 0xa2, 0x01, 0x00 }; // data taken from Nairol's captures uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x07, 0x00, 0xf4, 0x01, 0xb5, 0xa2, 0x01, 0x00 }; - //r = update_feature_report( sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) ); - r = update_feature_report(sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof(vive_controller_haptic_pulse)); + r = update_feature_report( sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) ); + r = getupdate_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)) printf("HAPTIC FAILED **************************\n"); // return 5; OGUSleep(1000); } -//#endif +#endif #if 0 - // working code to turn off a wireless controller: - { - uint8_t vive_controller_off[64] = { 0xff, 0x9f, 0x04, 'o', 'f', 'f', '!' }; - //r = update_feature_report( sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) ); - r = update_feature_report(sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_off, sizeof(vive_controller_off)); - SV_INFO("UCR: %d", r); - if (r != sizeof(vive_controller_off)) printf("OFF FAILED **************************\n"); // return 5; - OGUSleep(1000); - } + //// working code to turn off a wireless controller: + //{ + // uint8_t vive_controller_off[64] = { 0xff, 0x9f, 0x04, 'o', 'f', 'f', '!' }; + // //r = update_feature_report( sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) ); + // r = update_feature_report(sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_off, sizeof(vive_controller_off)); + // SV_INFO("UCR: %d", r); + // if (r != sizeof(vive_controller_off)) printf("OFF FAILED **************************\n"); // return 5; + // OGUSleep(1000); + //} #endif //if (sv->udev[USB_DEV_TRACKER0]) //{ -- cgit v1.2.3 From 499b80ae7b538f8e66f5ec8bfa60c7136a3babf5 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Thu, 4 Jan 2018 08:47:42 -0700 Subject: Haptic Call Plumbed The plumbing is now in place for the haptic call. Left in place a "demo" where haptic is called when a controller's trigger is pulled --- include/libsurvive/survive.h | 5 +++ include/libsurvive/survive_types.h | 2 + src/survive.c | 13 +++++- src/survive_process.c | 26 +++++++++--- src/survive_vive.c | 87 +++++++++++++++++++++++++++++++++----- 5 files changed, 115 insertions(+), 18 deletions(-) diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index e4afadf..0cfab1f 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -19,6 +19,7 @@ struct SurviveObject char codename[4]; //3 letters, null-terminated. Currently HMD, WM0, WM1. char drivername[4]; //3 letters for driver. Currently "HTC" + void *driver; int32_t buttonmask; int16_t axis1; @@ -68,6 +69,7 @@ struct SurviveObject FLT* gyro_bias; // size is FLT*3. contains x,y,z FLT* gyro_scale; // size is FLT*3. contains x,y,z + haptic_func haptic; //Debug int tsl; @@ -179,6 +181,9 @@ void survive_cal_install( SurviveContext * ctx ); //XXX This will be removed if // Read back a human-readable string description of the calibration status int survive_cal_get_status( struct SurviveContext * ctx, char * description, int description_length ); +// Induce haptic feedback +int survive_haptic(SurviveObject * so, uint8_t reserved, uint16_t pulseHigh, uint16_t pulseLow, uint16_t repeatCount); + //Call these from your callback if overridden. //Accept higher-level data. void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length , uint32_t lh); diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h index fa3eb2f..9a6e148 100644 --- a/include/libsurvive/survive_types.h +++ b/include/libsurvive/survive_types.h @@ -44,6 +44,8 @@ typedef void (*imu_process_func)( SurviveObject * so, int mask, FLT * accelgyro, typedef void (*angle_process_func)( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh); typedef void (*button_process_func)(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val); +typedef int(*haptic_func)(SurviveObject * so, uint8_t reserved, uint16_t pulseHigh , uint16_t pulseLow, uint16_t repeatCount); + //Device drivers (prefix your drivers with "DriverReg") i.e. // REGISTER_LINKTIME( DriverRegHTCVive ); typedef int (*DeviceDriver)( SurviveContext * ctx ); diff --git a/src/survive.c b/src/survive.c index d3c0918..75c07bf 100755 --- a/src/survive.c +++ b/src/survive.c @@ -60,7 +60,7 @@ static void button_servicer(void * context) // should never happen. indicates failure of code pushing stuff onto // the buttonQueue // if it does happen, it will kill all future button input - printf("ERROR: Unpopulated ButtonQueueEntry!"); + printf("ERROR: Unpopulated ButtonQueueEntry! NextReadIndex=%d\n", ctx->buttonQueue.nextReadIndex); return; } @@ -272,6 +272,17 @@ int survive_send_magic( SurviveContext * ctx, int magic_code, void * data, int d return 0; } +int survive_haptic(SurviveObject * so, uint8_t reserved, uint16_t pulseHigh, uint16_t pulseLow, uint16_t repeatCount) +{ + if (NULL == so || NULL == so->haptic) + { + return -404; + } + + return so->haptic(so, reserved, pulseHigh, pulseLow, repeatCount); +} + + void survive_close( SurviveContext * ctx ) { const char * DriverName; diff --git a/src/survive_process.c b/src/survive_process.c index eaed881..4b86144 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -66,13 +66,25 @@ void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val) { // do nothing. - printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n", - eventType, - buttonId, - axis1Id, - axis1Val, - axis2Id, - axis2Val); + //printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n", + // eventType, + // buttonId, + // axis1Id, + // axis1Val, + // axis2Id, + // axis2Val); + if (buttonId == 24 && eventType == 1) // trigger engage + { + for (int j = 0; j < 40; j++) + { + for (int i = 0; i < 0x1; i++) + { + survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100); + OGUSleep(5000); + } + OGUSleep(20000); + } + } } void survive_default_imu_process( SurviveObject * so, int mask, FLT * accelgyromag, uint32_t timecode, int id ) diff --git a/src/survive_vive.c b/src/survive_vive.c index 3df60ba..9e4a8b8 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -620,17 +620,24 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo //#endif #if 0 - for (int i = 0; i < 0xf; i++) + for (int j=0; j < 40; j++) { - //uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x7, 0 /*right*/, 0xFF /*period on*/, 0xFF/*period on*/, 0xFF/*period off*/, 0xFF/*period off*/, 0xFF /* repeat Count */, 0xFF /* repeat count */ }; - //uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x07, 0x00, 0xf4, 0x01, 0xb5, 0xa2, 0x01, 0x00 }; // data taken from Nairol's captures - uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x07, 0x00, 0xf4, 0x01, 0xb5, 0xa2, 0x01, 0x00 }; - r = update_feature_report( sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) ); - r = getupdate_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)) printf("HAPTIC FAILED **************************\n"); // return 5; - OGUSleep(1000); + for (int i = 0; i < 0x1; i++) + { + //uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x7, 0 /*right*/, 0xFF /*period on*/, 0xFF/*period on*/, 0xFF/*period off*/, 0xFF/*period off*/, 0xFF /* repeat Count */, 0xFF /* repeat count */ }; + //uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x07, 0x00, 0xf4, 0x01, 0xb5, 0xa2, 0x01, 0x00 }; // data taken from Nairol's captures + uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x07, 0x00, 0xf4, 0x01, 0xb5, 0xa2, 0x01* j, 0x00 }; + r = update_feature_report( sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) ); + r = getupdate_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)) printf("HAPTIC FAILED **************************\n"); // return 5; + OGUSleep(5000); + } + + OGUSleep(20000); } + + #endif #if 0 @@ -686,6 +693,54 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo return 0; } +int survive_vive_send_haptic(SurviveObject * so, uint8_t reserved, uint16_t pulseHigh, uint16_t pulseLow, uint16_t repeatCount) +{ + SurviveViveData *sv = (SurviveViveData*)so->driver; + + if (NULL == sv) + { + return -500; + } + + int r; + uint8_t vive_controller_haptic_pulse[64] = { + 0xff, 0x8f, 0x07, 0x00, + pulseHigh & 0xff00 >> 8, pulseHigh & 0xff, + pulseLow & 0xff00 >> 8, pulseLow & 0xff, + repeatCount & 0xff00 >> 8, repeatCount & 0xff, + }; + + r = update_feature_report(sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof(vive_controller_haptic_pulse)); + r = getupdate_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)) + { + printf("HAPTIC FAILED **************************\n"); + return -1; + } + + return 0; + + //for (int j = 0; j < 40; j++) + //{ + // for (int i = 0; i < 0x1; i++) + // { + // //uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x7, 0 /*right*/, 0xFF /*period on*/, 0xFF/*period on*/, 0xFF/*period off*/, 0xFF/*period off*/, 0xFF /* repeat Count */, 0xFF /* repeat count */ }; + // //uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x07, 0x00, 0xf4, 0x01, 0xb5, 0xa2, 0x01, 0x00 }; // data taken from Nairol's captures + // uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x07, 0x00, 0xf4, 0x01, 0xb5, 0xa2, 0x01 * j, 0x00 }; + // r = update_feature_report(sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof(vive_controller_haptic_pulse)); + // r = getupdate_feature_report(sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof(vive_controller_haptic_pulse)); + // if (r != sizeof(vive_controller_haptic_pulse)) printf("HAPTIC FAILED **************************\n"); // return 5; + // OGUSleep(5000); + // } + + // OGUSleep(20000); + //} + + ////OGUSleep(5000); + //return 0; +} + void survive_vive_usb_close( SurviveViveData * sv ) { int i; @@ -903,9 +958,10 @@ void incrementAndPostButtonQueue(SurviveContext *ctx) { ButtonQueueEntry *entry = &(ctx->buttonQueue.entry[ctx->buttonQueue.nextWriteIndex]); - if (OGGetSema(ctx->buttonQueue.buttonservicesem) >= BUTTON_QUEUE_MAX_LEN) + if (OGGetSema(ctx->buttonQueue.buttonservicesem) >= BUTTON_QUEUE_MAX_LEN-1) { // There's not enough space to write this entry. Clear it out and move along + //printf("Button Buffer Full\n"); memset(entry, 0, sizeof(ButtonQueueEntry)); return; } @@ -1759,6 +1815,9 @@ void init_SurviveObject(SurviveObject* so) { so->acc_bias = NULL; so->gyro_scale = NULL; so->gyro_bias = NULL; + so->haptic = NULL; + so->PoserData = NULL; + so->disambiguator_data = NULL; } int DriverRegHTCVive( SurviveContext * ctx ) @@ -1789,18 +1848,23 @@ int DriverRegHTCVive( SurviveContext * ctx ) hmd->ctx = ctx; + hmd->driver = sv; memcpy( hmd->codename, "HMD", 4 ); memcpy( hmd->drivername, "HTC", 4 ); wm0->ctx = ctx; + wm0->driver = sv; memcpy( wm0->codename, "WM0", 4 ); memcpy( wm0->drivername, "HTC", 4 ); wm1->ctx = ctx; + wm1->driver = sv; memcpy( wm1->codename, "WM1", 4 ); memcpy( wm1->drivername, "HTC", 4 ); tr0->ctx = ctx; + tr0->driver = sv; memcpy( tr0->codename, "TR0", 4 ); memcpy( tr0->drivername, "HTC", 4 ); ww0->ctx = ctx; + ww0->driver = sv; memcpy( ww0->codename, "WW0", 4 ); memcpy( ww0->drivername, "HTC", 4 ); @@ -1844,6 +1908,9 @@ int DriverRegHTCVive( SurviveContext * ctx ) wm1->timecenter_ticks = wm1->timebase_hz / 240; tr0->timecenter_ticks = tr0->timebase_hz / 240; ww0->timecenter_ticks = ww0->timebase_hz / 240; + + wm0->haptic = survive_vive_send_haptic; + wm1->haptic = survive_vive_send_haptic; /* int i; int locs = hmd->nr_locations; -- cgit v1.2.3 From 735a8bd11070b0c563e891ff8b70ce297a52a367 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Fri, 5 Jan 2018 03:54:29 -0700 Subject: Add standard output mechanism for posers Added a raw pose output/ callback that the posers can call when they have calculated a pose. --- include/libsurvive/survive.h | 5 ++++- include/libsurvive/survive_types.h | 3 ++- src/poser_turveytori.c | 8 ++++++-- src/survive.c | 9 ++++++++- src/survive_data.c | 4 ++-- src/survive_process.c | 39 +++++++++++++++++++++++++++++--------- 6 files changed, 52 insertions(+), 16 deletions(-) diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 0cfab1f..4821e63 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -129,6 +129,7 @@ struct SurviveContext imu_process_func imuproc; angle_process_func angleproc; button_process_func buttonproc; + raw_pose_func rawposeproc; struct config_group* global_config_values; struct config_group* lh_config; //lighthouse configs @@ -163,7 +164,8 @@ void survive_install_error_fn( SurviveContext * ctx, text_feedback_func fbp ); void survive_install_light_fn( SurviveContext * ctx, light_process_func fbp ); void survive_install_imu_fn( SurviveContext * ctx, imu_process_func fbp ); void survive_install_angle_fn( SurviveContext * ctx, angle_process_func fbp ); -void survive_install_button_fn( SurviveContext * ctx, button_process_func fbp ); +void survive_install_button_fn(SurviveContext * ctx, button_process_func fbp); +void survive_install_raw_pose_fn(SurviveContext * ctx, raw_pose_func fbp); void survive_close( SurviveContext * ctx ); int survive_poll( SurviveContext * ctx ); @@ -190,6 +192,7 @@ void survive_default_light_process( SurviveObject * so, int sensor_id, int acode void survive_default_imu_process( SurviveObject * so, int mode, FLT * accelgyro, uint32_t timecode, int id ); void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh ); void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val); +void survive_default_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *position, FLT *quaternion); ////////////////////// Survive Drivers //////////////////////////// diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h index 9a6e148..5384345 100644 --- a/include/libsurvive/survive_types.h +++ b/include/libsurvive/survive_types.h @@ -42,7 +42,8 @@ typedef void (*text_feedback_func)( SurviveContext * ctx, const char * fault ); typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lighthouse); typedef void (*imu_process_func)( SurviveObject * so, int mask, FLT * accelgyro, uint32_t timecode, int id ); typedef void (*angle_process_func)( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh); -typedef void (*button_process_func)(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val); +typedef void(*button_process_func)(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val); +typedef void(*raw_pose_func)(SurviveObject * so, uint8_t lighthouse, FLT *position, FLT *quaternion); typedef int(*haptic_func)(SurviveObject * so, uint8_t reserved, uint16_t pulseHigh , uint16_t pulseLow, uint16_t repeatCount); diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index ac979c7..28e533e 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1177,7 +1177,7 @@ static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *in } - printf("Ri=%3d Fitness=%3f ", i, lastMatchFitness); + if (ttDebug) printf("Ri=%3d Fitness=%3f ", i, lastMatchFitness); } @@ -1638,7 +1638,11 @@ static void QuickPose(SurviveObject *so, int lh) SolveForLighthouse(pos, quat, to, so, 0, lh, 0); - printf("P&O: [% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]); + //printf("P&O: [% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]); + if (so->ctx->rawposeproc) + { + so->ctx->rawposeproc(so, lh, pos, quat); + } if (ttDebug) printf("!\n"); } diff --git a/src/survive.c b/src/survive.c index 75c07bf..1c406e8 100755 --- a/src/survive.c +++ b/src/survive.c @@ -184,6 +184,7 @@ SurviveContext * survive_init( int headless ) // start the thread to process button data ctx->buttonservicethread = OGCreateThread(button_servicer, ctx); survive_install_button_fn(ctx, NULL); + survive_install_raw_pose_fn(ctx, NULL); return ctx; } @@ -235,9 +236,15 @@ void survive_install_button_fn(SurviveContext * ctx, button_process_func fbp) ctx->buttonproc = fbp; else ctx->buttonproc = survive_default_button_process; - } +void survive_install_raw_pose_fn(SurviveContext * ctx, raw_pose_func fbp) +{ + if (fbp) + ctx->rawposeproc = fbp; + else + ctx->rawposeproc = survive_default_raw_pose_process; +} int survive_add_object( SurviveContext * ctx, SurviveObject * obj ) { int oldct = ctx->objs_ct; diff --git a/src/survive_data.c b/src/survive_data.c index 1b80269..0427659 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -419,8 +419,8 @@ void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le ) } if (lcd->per_sweep.activeLighthouse < 0) { - fprintf(stderr, "WARNING: No active lighthouse!\n"); - fprintf(stderr, " %2d %8d %d %d\n", le->sensor_id, le->length,lcd->per_sweep.lh_acode[0],lcd->per_sweep.lh_acode[1]); + //fprintf(stderr, "WARNING: No active lighthouse!\n"); + //fprintf(stderr, " %2d %8d %d %d\n", le->sensor_id, le->length,lcd->per_sweep.lh_acode[0],lcd->per_sweep.lh_acode[1]); return; } diff --git a/src/survive_process.c b/src/survive_process.c index 4b86144..42d2897 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -66,20 +66,34 @@ void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val) { // do nothing. - //printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n", - // eventType, - // buttonId, - // axis1Id, - // axis1Val, - // axis2Id, - // axis2Val); + printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n", + eventType, + buttonId, + axis1Id, + axis1Val, + axis2Id, + axis2Val); if (buttonId == 24 && eventType == 1) // trigger engage { - for (int j = 0; j < 40; j++) + for (int j = 0; j < 6; j++) { - for (int i = 0; i < 0x1; i++) + for (int i = 0; i < 0x5; i++) { survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100); + //survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100); + OGUSleep(1000); + } + OGUSleep(20000); + } + } + if (buttonId == 2 && eventType == 1) // trigger engage + { + for (int j = 0; j < 6; j++) + { + for (int i = 0; i < 0x1; i++) + { + survive_haptic(so, 0, 0xf401, 0x05a2, 0xf100); + //survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100); OGUSleep(5000); } OGUSleep(20000); @@ -87,6 +101,13 @@ void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8 } } +void survive_default_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *pos, FLT *quat) +{ + // print the pose; + printf("Pose: [%2.2x][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]); + +} + void survive_default_imu_process( SurviveObject * so, int mask, FLT * accelgyromag, uint32_t timecode, int id ) { if( so->PoserFn ) -- cgit v1.2.3 From ed85661121c3ed8769a69d059fd3162aec33c6fe Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Fri, 5 Jan 2018 04:09:54 -0700 Subject: Update test.c to do calibration --- src/poser_turveytori.c | 12 ++++++++++++ src/survive_process.c | 2 +- test.c | 2 ++ 3 files changed, 15 insertions(+), 1 deletion(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 28e533e..ad30f6c 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1856,6 +1856,18 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) // } //} + for (int i=0; i < ctx->activeLighthouses; i++) + { + printf("Lighthouse Pose: [%1.1x][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", + i, + ctx->bsd[i].Pose.Pos[0], + ctx->bsd[i].Pose.Pos[1], + ctx->bsd[i].Pose.Pos[2], + ctx->bsd[i].Pose.Rot[0], + ctx->bsd[i].Pose.Rot[1], + ctx->bsd[i].Pose.Rot[2], + ctx->bsd[i].Pose.Rot[3]); + } config_set_lighthouse(ctx->lh_config, ctx->bsd[0], 0); config_set_lighthouse(ctx->lh_config, ctx->bsd[1], 1); diff --git a/src/survive_process.c b/src/survive_process.c index 42d2897..1df24e9 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -104,7 +104,7 @@ void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8 void survive_default_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *pos, FLT *quat) { // print the pose; - printf("Pose: [%2.2x][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]); + printf("Pose: [%1.1x][%s][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, so->codename, pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]); } diff --git a/test.c b/test.c index 4909d50..c9f03d7 100644 --- a/test.c +++ b/test.c @@ -85,6 +85,8 @@ int main() dump_iface( survive_get_so_by_name( ctx, "TR0" ), "TR0" ); dump_iface( survive_get_so_by_name( ctx, "WW0" ), "WW0" ); + survive_cal_install(ctx); + while(survive_poll(ctx) == 0) { double Now = OGGetAbsoluteTime(); -- cgit v1.2.3 From deb657e885abc0abd98dbeee0a74f56f64d46754 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Fri, 5 Jan 2018 04:48:12 -0700 Subject: Remove dependency on GetSem to fix linux segfault Longer term, need to determine why if failed and fix root cause. --- src/survive_vive.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/survive_vive.c b/src/survive_vive.c index 9e4a8b8..65636cf 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -958,7 +958,7 @@ void incrementAndPostButtonQueue(SurviveContext *ctx) { ButtonQueueEntry *entry = &(ctx->buttonQueue.entry[ctx->buttonQueue.nextWriteIndex]); - if (OGGetSema(ctx->buttonQueue.buttonservicesem) >= BUTTON_QUEUE_MAX_LEN-1) + if ((ctx->buttonQueue.nextWriteIndex+1)% BUTTON_QUEUE_MAX_LEN == ctx->buttonQueue.nextReadIndex) { // There's not enough space to write this entry. Clear it out and move along //printf("Button Buffer Full\n"); -- cgit v1.2.3 From e642395e61691a6dab749c2085410117d88956f5 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Sat, 6 Jan 2018 11:23:07 -0700 Subject: A few changes to test.c Hopefully, this will make it easier for anyone wanting to use libsurvive to have a simple starting point for doing so. --- src/survive_process.c | 68 ++++++++++++++++++++++---------------------- test.c | 79 +++++++++++++++++++++++++++++++++++++++++++++++---- 2 files changed, 107 insertions(+), 40 deletions(-) diff --git a/src/survive_process.c b/src/survive_process.c index 1df24e9..0f19007 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -66,45 +66,45 @@ void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val) { // do nothing. - printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n", - eventType, - buttonId, - axis1Id, - axis1Val, - axis2Id, - axis2Val); - if (buttonId == 24 && eventType == 1) // trigger engage - { - for (int j = 0; j < 6; j++) - { - for (int i = 0; i < 0x5; i++) - { - survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100); - //survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100); - OGUSleep(1000); - } - OGUSleep(20000); - } - } - if (buttonId == 2 && eventType == 1) // trigger engage - { - for (int j = 0; j < 6; j++) - { - for (int i = 0; i < 0x1; i++) - { - survive_haptic(so, 0, 0xf401, 0x05a2, 0xf100); - //survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100); - OGUSleep(5000); - } - OGUSleep(20000); - } - } + //printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n", + // eventType, + // buttonId, + // axis1Id, + // axis1Val, + // axis2Id, + // axis2Val); + //if (buttonId == 24 && eventType == 1) // trigger engage + //{ + // for (int j = 0; j < 6; j++) + // { + // for (int i = 0; i < 0x5; i++) + // { + // survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100); + // //survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100); + // OGUSleep(1000); + // } + // OGUSleep(20000); + // } + //} + //if (buttonId == 2 && eventType == 1) // trigger engage + //{ + // for (int j = 0; j < 6; j++) + // { + // for (int i = 0; i < 0x1; i++) + // { + // survive_haptic(so, 0, 0xf401, 0x05a2, 0xf100); + // //survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100); + // OGUSleep(5000); + // } + // OGUSleep(20000); + // } + //} } void survive_default_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *pos, FLT *quat) { // print the pose; - printf("Pose: [%1.1x][%s][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, so->codename, pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]); + //printf("Pose: [%1.1x][%s][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, so->codename, pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]); } diff --git a/test.c b/test.c index c9f03d7..5fc62af 100644 --- a/test.c +++ b/test.c @@ -37,6 +37,69 @@ void HandleDestroy() { } +void testprog_button_process(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val) +{ + survive_default_button_process(so, eventType, buttonId, axis1Id, axis1Val, axis2Id, axis2Val); + + // do nothing. + printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n", + eventType, + buttonId, + axis1Id, + axis1Val, + axis2Id, + axis2Val); + + // Note: the code for haptic feedback is not yet written to support wired USB connections to the controller. + // Work is still needed to reverse engineer that USB protocol. + + // let's do some haptic feedback if the trigger is pushed + if (buttonId == 24 && eventType == 1) // trigger engage + { + for (int j = 0; j < 6; j++) + { + for (int i = 0; i < 0x5; i++) + { + survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100); + //survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100); + OGUSleep(1000); + } + OGUSleep(20000); + } + } + + // let's do some haptic feedback if the touchpad is pressed. + if (buttonId == 2 && eventType == 1) // trigger engage + { + for (int j = 0; j < 6; j++) + { + for (int i = 0; i < 0x1; i++) + { + survive_haptic(so, 0, 0xf401, 0x05a2, 0xf100); + //survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100); + OGUSleep(5000); + } + OGUSleep(20000); + } + } +} + +void testprog_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *pos, FLT *quat) +{ + survive_default_raw_pose_process(so, lighthouse, pos, quat); + + // print the pose; + printf("Pose: [%1.1x][%s][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, so->codename, pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]); +} + +void testprog_imu_process(SurviveObject * so, int mask, FLT * accelgyromag, uint32_t timecode, int id) +{ + survive_default_imu_process(so, mask, accelgyromag, timecode, id); + + // so something here, like printing out the data... + +} + static void dump_iface( struct SurviveObject * so, const char * prefix ) { @@ -73,6 +136,10 @@ int main() ctx = survive_init( 0 ); + survive_install_button_fn(ctx, testprog_button_process); + survive_install_raw_pose_fn(ctx, testprog_raw_pose_process); + survive_install_imu_fn(ctx, testprog_imu_process); + if( !ctx ) { fprintf( stderr, "Fatal. Could not start\n" ); @@ -89,12 +156,12 @@ int main() while(survive_poll(ctx) == 0) { - double Now = OGGetAbsoluteTime(); - if( Now > (Start+1) && !magicon ) - { - survive_send_magic(ctx,1,0,0); - magicon = 1; - } + //double Now = OGGetAbsoluteTime(); + //if( Now > (Start+1) && !magicon ) + //{ + // survive_send_magic(ctx,1,0,0); + // magicon = 1; + //} //Do stuff. } } -- cgit v1.2.3 From 91fc90bf9171fe588fd11492bdae7419d5ccd921 Mon Sep 17 00:00:00 2001 From: Christoph Haag Date: Mon, 15 Jan 2018 17:10:15 +0100 Subject: fix segfault in config_set_lighthouse() also include the survive_config.h header to avoid various implicit function declaration warnings on gcc --- src/poser_turveytori.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index ad30f6c..59ff25e 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1,4 +1,5 @@ #include +#include #include #include #include @@ -1868,8 +1869,8 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) ctx->bsd[i].Pose.Rot[2], ctx->bsd[i].Pose.Rot[3]); } - config_set_lighthouse(ctx->lh_config, ctx->bsd[0], 0); - config_set_lighthouse(ctx->lh_config, ctx->bsd[1], 1); + config_set_lighthouse(ctx->lh_config, &ctx->bsd[0], 0); + config_set_lighthouse(ctx->lh_config, &ctx->bsd[1], 1); config_save(ctx, "config.json"); -- cgit v1.2.3 From c570809ce1d1cc1d30ef4db547e6388b3fd80ac1 Mon Sep 17 00:00:00 2001 From: Christoph Haag Date: Mon, 15 Jan 2018 17:10:55 +0100 Subject: fix various -Wall warnings --- calibrate.c | 2 +- src/poser_octavioradii.c | 6 +++--- src/survive.c | 2 +- src/survive_vive.c | 4 ++-- 4 files changed, 7 insertions(+), 7 deletions(-) diff --git a/calibrate.c b/calibrate.c index cfd3e17..ff3b774 100644 --- a/calibrate.c +++ b/calibrate.c @@ -14,7 +14,7 @@ struct SurviveContext * ctx; int quit = 0; -static LighthouseCount = 0; +static int LighthouseCount = 0; void HandleKey( int keycode, int bDown ) { diff --git a/src/poser_octavioradii.c b/src/poser_octavioradii.c index 0d8674c..be1e9f7 100644 --- a/src/poser_octavioradii.c +++ b/src/poser_octavioradii.c @@ -252,7 +252,7 @@ static void normalizeAndMultiplyVector(FLT *vectorToNormalize, size_t count, FLT } -static RefineEstimateUsingGradientDescentRadii(FLT *estimateOut, SensorAngles *angles, FLT *initialEstimate, size_t numRadii, PointPair *pairs, size_t numPairs, FILE *logFile) +static void RefineEstimateUsingGradientDescentRadii(FLT *estimateOut, SensorAngles *angles, FLT *initialEstimate, size_t numRadii, PointPair *pairs, size_t numPairs, FILE *logFile) { int i = 0; FLT lastMatchFitness = calculateFitness(angles, initialEstimate, pairs, numPairs); @@ -521,9 +521,9 @@ static void QuickPose(SurviveObject *so) if (sensorCount > 4) { - FLT pos[3]; + Point pos; FLT orient[4]; - SolveForLighthouseRadii(pos, orient, to); + SolveForLighthouseRadii(&pos, orient, to); } diff --git a/src/survive.c b/src/survive.c index 1c406e8..a5ca68f 100755 --- a/src/survive.c +++ b/src/survive.c @@ -182,7 +182,7 @@ SurviveContext * survive_init( int headless ) ctx->buttonQueue.buttonservicesem = OGCreateSema(); // start the thread to process button data - ctx->buttonservicethread = OGCreateThread(button_servicer, ctx); + ctx->buttonservicethread = OGCreateThread(&button_servicer, ctx); survive_install_button_fn(ctx, NULL); survive_install_raw_pose_fn(ctx, NULL); diff --git a/src/survive_vive.c b/src/survive_vive.c index 65636cf..934b5d3 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -309,8 +309,8 @@ static inline int getupdate_feature_report(libusb_device_handle* dev, uint16_t i 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; + if (ret < 0) + return -1; return ret; } -- cgit v1.2.3 From 7817da63526f35d10d6d4f6b3d9c02280719e023 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Mon, 15 Jan 2018 21:50:10 -0700 Subject: Fix compiler warnings --- include/libsurvive/survive.h | 5 ++--- redist/json_helpers.c | 2 +- src/poser_octavioradii.c | 2 +- src/poser_turveytori.c | 5 ++--- src/survive.c | 10 ++++++---- src/survive_config.c | 2 +- 6 files changed, 13 insertions(+), 13 deletions(-) diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 4821e63..3d0d472 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -4,7 +4,6 @@ #include #include "survive_types.h" #include "poser.h" -#include "os_generic.h" #ifdef __cplusplus extern "C" { @@ -117,7 +116,7 @@ typedef struct { uint8_t nextReadIndex; //init to 0 uint8_t nextWriteIndex; // init to 0 - og_sema_t buttonservicesem; + void* buttonservicesem; ButtonQueueEntry entry[BUTTON_QUEUE_MAX_LEN]; } ButtonQueue; @@ -150,7 +149,7 @@ struct SurviveContext uint8_t isClosing; // flag to indicate if threads should terminate themselves - og_thread_t buttonservicethread; + void* buttonservicethread; ButtonQueue buttonQueue; }; diff --git a/redist/json_helpers.c b/redist/json_helpers.c index af7ddda..8da841b 100644 --- a/redist/json_helpers.c +++ b/redist/json_helpers.c @@ -121,7 +121,7 @@ char* load_file_to_mem(const char* path) { fseek( f, 0, SEEK_SET ); char * JSON_STRING = malloc( len + 1); memset(JSON_STRING,0,len+1); - int i = fread( JSON_STRING, len, 1, f ); i = i; //Ignore return value. + size_t i = fread( JSON_STRING, len, 1, f ); i = i; //Ignore return value. fclose( f ); return JSON_STRING; } diff --git a/src/poser_octavioradii.c b/src/poser_octavioradii.c index be1e9f7..5805e1a 100644 --- a/src/poser_octavioradii.c +++ b/src/poser_octavioradii.c @@ -591,7 +591,7 @@ int PoserOctavioRadii( SurviveObject * so, PoserData * pd ) } else { - dd->hitCount[i][l->lh][axis] *= 0.5; + dd->hitCount[i][l->lh][axis] = (int)((double)dd->hitCount[i][l->lh][axis] * 0.5); } } //if (0 == l->lh) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 59ff25e..de1516a 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1,5 +1,5 @@ #include -#include +#include "survive_config.h" #include #include #include @@ -785,7 +785,6 @@ FLT RotationEstimateFitnessQuaternion(Point lhPoint, FLT *quaternion, TrackedObj FLT throwaway = RotationEstimateFitnessAxisAngle(lhPoint, axisAngle, obj); - int a = throwaway; return throwaway; } @@ -1397,7 +1396,7 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob SolveForRotationQuat(rotQuat, obj, refinedEstimateGd); SolveForRotation(rot, obj, refinedEstimateGd); FLT objPos[3]; - FLT objPos2[3]; + //FLT objPos2[3]; //{ // toriData->lastLhRotQuat[lh][0] = rotQuat[0]; diff --git a/src/survive.c b/src/survive.c index a5ca68f..0386275 100755 --- a/src/survive.c +++ b/src/survive.c @@ -8,6 +8,7 @@ #include #include "survive_config.h" +#include "os_generic.h" #ifdef __APPLE__ #define z_const const @@ -40,7 +41,7 @@ static void survivenote( struct SurviveContext * ctx, const char * fault ) fprintf( stderr, "Info: %s\n", fault ); } -static void button_servicer(void * context) +static void *button_servicer(void * context) { SurviveContext *ctx = (SurviveContext*)context; @@ -51,7 +52,7 @@ static void button_servicer(void * context) if (ctx->isClosing) { // we're shutting down. Close. - return; + return NULL; } ButtonQueueEntry *entry = &(ctx->buttonQueue.entry[ctx->buttonQueue.nextReadIndex]); @@ -61,7 +62,7 @@ static void button_servicer(void * context) // the buttonQueue // if it does happen, it will kill all future button input printf("ERROR: Unpopulated ButtonQueueEntry! NextReadIndex=%d\n", ctx->buttonQueue.nextReadIndex); - return; + return NULL; } //printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n", @@ -90,6 +91,7 @@ static void button_servicer(void * context) ctx->buttonQueue.nextReadIndex = 0; } }; + return NULL; } SurviveContext * survive_init( int headless ) @@ -182,7 +184,7 @@ SurviveContext * survive_init( int headless ) ctx->buttonQueue.buttonservicesem = OGCreateSema(); // start the thread to process button data - ctx->buttonservicethread = OGCreateThread(&button_servicer, ctx); + ctx->buttonservicethread = OGCreateThread(button_servicer, ctx); survive_install_button_fn(ctx, NULL); survive_install_raw_pose_fn(ctx, NULL); diff --git a/src/survive_config.c b/src/survive_config.c index 0961651..46c6658 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -187,7 +187,7 @@ uint16_t config_read_float_array(config_group *cg, const char *tag, FLT* values, config_entry *cv = find_config_entry(cg, tag); if (cv != NULL) { - for (int i=0; i < CFG_MIN(count, cv->elements); i++) + for (unsigned int i=0; i < CFG_MIN(count, cv->elements); i++) { values[i] = ((double*)cv->data)[i]; } -- cgit v1.2.3 From b2eb7569a8963917116c4520e15b17f0578a2509 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Mon, 15 Jan 2018 22:40:14 -0700 Subject: Fix a few warnings --- redist/CNFGWinDriver.c | 5 ++--- redist/json_helpers.c | 2 +- src/ootx_decoder.c | 2 +- src/poser_turveytori.c | 5 +++-- 4 files changed, 7 insertions(+), 7 deletions(-) diff --git a/redist/CNFGWinDriver.c b/redist/CNFGWinDriver.c index b1c1eb0..9d1be6a 100644 --- a/redist/CNFGWinDriver.c +++ b/redist/CNFGWinDriver.c @@ -54,8 +54,8 @@ void CNFGGetDimensions( short * x, short * y ) lasty = buffery; InternalHandleResize(); } - *x = bufferx; - *y = buffery; + *x = (short)bufferx; + *y = (short)buffery; } @@ -212,7 +212,6 @@ void CNFGSetup( const char * name_of_window, int width, int height ) void CNFGHandleInput() { - int ldown = 0; MSG msg; while( PeekMessage( &msg, lsHWND, 0, 0xFFFF, 1 ) ) diff --git a/redist/json_helpers.c b/redist/json_helpers.c index 8da841b..109708a 100644 --- a/redist/json_helpers.c +++ b/redist/json_helpers.c @@ -193,7 +193,7 @@ void json_load_file(const char* path) { } else if (value_t->type == JSMN_OBJECT) { printf("Begin Object\n"); if (json_begin_object != NULL) json_begin_object(tag); - children = value_t->size +1; //+1 to account for this loop where we are not yed parsing children + children = (int16_t)(value_t->size +1); //+1 to account for this loop where we are not yed parsing children // i += decode_jsmn_object(JSON_STRING, tokens+i+2,value_t->size); } else { diff --git a/src/ootx_decoder.c b/src/ootx_decoder.c index f7a7938..7bf7d7e 100644 --- a/src/ootx_decoder.c +++ b/src/ootx_decoder.c @@ -44,7 +44,7 @@ void ootx_free_decoder_context(ootx_decoder_context *ctx) { } uint8_t ootx_decode_bit(uint32_t length) { - uint8_t t = (length - 2750) / 500; //why 2750? + uint8_t t = (uint8_t)((length - 2750) / 500); //why 2750? // return ((t & 0x02)>0)?0xFF:0x00; //easier if we need to bitshift right return ((t & 0x02)>>1); } diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index de1516a..94d572e 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1050,7 +1050,7 @@ static void WhereIsTheTrackedObjectQuaternion(FLT *posOut, FLT *rotation, Point quatgetreciprocal(inverseRotation, rotation); - FLT objPoint[3] = { lhPoint.x, lhPoint.y, lhPoint.z }; + //FLT objPoint[3] = { lhPoint.x, lhPoint.y, lhPoint.z }; //rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]); quatrotatevector(posOut, inverseRotation, posOut); @@ -1186,7 +1186,7 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) // Step 1, create initial quaternion for guess. // This should have the lighthouse directly facing the tracked object. - Point trackedObjRelativeToLh = { .x = -lh.x,.y = -lh.y,.z = -lh.z }; + //Point trackedObjRelativeToLh = { .x = -lh.x,.y = -lh.y,.z = -lh.z }; FLT theta = atan2(-lh.x, -lh.y); FLT zAxis[4] = { 0, 0, 1 , theta - LINMATHPI / 2 }; FLT quat1[4]; @@ -1200,6 +1200,7 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) RefineRotationEstimateAxisAngle(rotOut, lh, zAxis, obj); + // TODO: Need to use the quaternion version here!!! //// Step 2, optimize the quaternion to match the data. //RefineRotationEstimateQuaternion(rotOut, lh, quat1, obj); -- cgit v1.2.3 From c9fec03986e23958e0dee3f0d1749fa8f5769ba7 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Tue, 16 Jan 2018 20:37:56 -0700 Subject: Add user pointer to survive context --- include/libsurvive/survive.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 3d0d472..30f5817 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -152,6 +152,8 @@ struct SurviveContext void* buttonservicethread; ButtonQueue buttonQueue; + void *user_ptr; + }; SurviveContext * survive_init( int headless ); -- cgit v1.2.3 From 0f206272d9338fdf10705b456b5af0539ee889bb Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Fri, 23 Feb 2018 22:57:59 -0700 Subject: Fixed typo which broke tr0 callbacks --- src/survive_vive.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/survive_vive.c b/src/survive_vive.c index 934b5d3..843482a 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -513,7 +513,7 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject * #else if( sv->udev[USB_DEV_HMD_IMU_LH] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_HMD_IMU_LH], 0x82, survive_data_cb, "Lightcap")) { return -12; } - if( sv->udev[USB_DEV_TRACKER0] && AttachInterface( sv, ww0, USB_IF_TRACKER0_LIGHTCAP, sv->udev[USB_DEV_TRACKER0], 0x82, survive_data_cb, "Tracker 0 Lightcap")) { return -13; } + if( sv->udev[USB_DEV_TRACKER0] && AttachInterface( sv, tr0, USB_IF_TRACKER0_LIGHTCAP, sv->udev[USB_DEV_TRACKER0], 0x82, survive_data_cb, "Tracker 0 Lightcap")) { return -13; } if( sv->udev[USB_DEV_W_WATCHMAN1] && AttachInterface( sv, ww0, USB_IF_W_WATCHMAN1_LIGHTCAP, sv->udev[USB_DEV_W_WATCHMAN1], 0x82, survive_data_cb, "Wired Watchman 1 Lightcap")) { return -13; } #endif SV_INFO( "All enumerated devices attached." ); -- cgit v1.2.3 From 9a9a539396c622246a527ed0c12df36edf5ff807 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Fri, 23 Feb 2018 23:15:31 -0700 Subject: Fixed unaligned access issues --- src/ootx_decoder.c | 7 ++++++- src/survive_vive.c | 18 +++++++++++++----- 2 files changed, 19 insertions(+), 6 deletions(-) diff --git a/src/ootx_decoder.c b/src/ootx_decoder.c index 7bf7d7e..ad55f5b 100644 --- a/src/ootx_decoder.c +++ b/src/ootx_decoder.c @@ -182,8 +182,13 @@ union iFloat { float f; }; + +struct __attribute__((__packed__)) unaligned_16_t { + uint16_t v; +}; + float _half_to_float(uint8_t* data) { - uint16_t x = *(uint16_t*)data; + uint16_t x = ((struct unaligned_16_t*)data)->v; union iFloat fnum; fnum.f = 0; diff --git a/src/survive_vive.c b/src/survive_vive.c index 934b5d3..a74d699 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -902,8 +902,16 @@ int survive_get_config( char ** config, SurviveViveData * sv, int devno, int ifa /////////////////////////////////////////////////////////////////////////////// #define POP1 (*(readdata++)) -#define POP2 (*(((uint16_t*)((readdata+=2)-2)))) -#define POP4 (*(((uint32_t*)((readdata+=4)-4)))) + + +struct __attribute__((__packed__)) unaligned_16_t { + uint16_t v; +}; +struct __attribute__((__packed__)) unaligned_32_t { + uint32_t v; +}; +#define POP2 ((((( struct unaligned_16_t*)((readdata+=2)-2))))->v) +#define POP4 ((((( struct unaligned_32_t*)((readdata+=4)-4))))->v) void calibrate_acc(SurviveObject* so, FLT* agm) { if (so->acc_bias != NULL) { @@ -1431,7 +1439,7 @@ void survive_data_cb( SurviveUSBInterface * si ) //printf( "%d -> ", size ); for( i = 0; i < 3; i++ ) { - int16_t * acceldata = (int16_t*)readdata; + struct unaligned_16_t * acceldata = (struct unaligned_16_t *)readdata; readdata += 12; uint32_t timecode = POP4; uint8_t code = POP1; @@ -1443,8 +1451,8 @@ void survive_data_cb( SurviveUSBInterface * si ) obj->oldcode = code; //XXX XXX BIG TODO!!! Actually recal gyro data. - FLT agm[9] = { acceldata[0], acceldata[1], acceldata[2], - acceldata[3], acceldata[4], acceldata[5], + FLT agm[9] = { acceldata[0].v, acceldata[1].v, acceldata[2].v, + acceldata[3].v, acceldata[4].v, acceldata[5].v, 0,0,0 }; agm[0]*=(float)(1./8192.0); -- cgit v1.2.3 From dabb0764a080ad219e1bb477e8cf7386a1a4dc7c Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Fri, 23 Feb 2018 23:22:28 -0700 Subject: Fixed OOB memory access --- src/survive_cal.c | 1 + src/survive_cal.h | 6 +++--- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/survive_cal.c b/src/survive_cal.c index 87d8c0b..f3a682a 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -15,6 +15,7 @@ #include #include #include +#include #include "survive_config.h" diff --git a/src/survive_cal.h b/src/survive_cal.h index ae644d1..f5ef6dc 100644 --- a/src/survive_cal.h +++ b/src/survive_cal.h @@ -51,14 +51,14 @@ struct SurviveCalData ootx_decoder_context ootx_decoders[NUM_LIGHTHOUSES]; //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]; + FLT all_lengths[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES][2][DRPTS + 1]; + FLT all_angles[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES][2][DRPTS + 1]; int16_t all_counts[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES][2]; int16_t peak_counts; int8_t found_common; int8_t times_found_common; - FLT all_sync_times[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES][DRPTS]; + FLT all_sync_times[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES][DRPTS + 1]; int16_t all_sync_counts[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES]; //For camfind (4+) -- cgit v1.2.3 From cee0ef5f2788589f31f08a470237bbaacf8f2665 Mon Sep 17 00:00:00 2001 From: Christoph Haag Date: Sun, 25 Feb 2018 19:16:30 +0100 Subject: add usage information to readme --- README.md | 135 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 134 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 51213dc..b587d3b 100644 --- a/README.md +++ b/README.md @@ -135,7 +135,140 @@ The limiting factor for Vive viability on a given computer is the maximum availa To support the Vive on HDMI, you either need a newer version of HDMI, or you need to define a custom resolution that respects pixel clock and video port limits, and is also accepted and displayed by the Vive. So far, we have not had success using custom resolutions on linux or on Windows. Windows imposes additional limitations in the form of restriction of WHQL certified drivers forbidden from using custom display resolutions (only allowing those defined by EDID in the monitor). Intel has released uncertified beta drivers for Haswell and newer processors, which should be able to support custom resolutions for the Vive (untested at this time). - +# Getting Started + +## General Information + +The default configuration of libsurvive requires both basestations and both controllers to be active, but currently libsurvive can not make proper use of both basestations at the same time - it outputs a different pose for each basestation and does not fuse the data for one more accurate pose. This will hopefully be implemented soon. For now it's a good idea to change the configuration to only require one basestation. + +Here is an example of a default configuration file that libsurvive will create as `config.json` in the current working directory when any libsurvive client is executed: + +``` +"LighthouseCount":"2", +"DefaultPoser":"PoserTurveyTori", +"RequiredTrackersForCal":"", +"AllowAllTrackersForCal":"1", +"ConfigPoser":"PoserTurveyTori", +"TurveyToriDebug":"0" +"lighthouse0":{ +"index":"0", +"id":"138441170", +"pose":["0.000000","0.000000","0.000000","0.000000","0.000000","0.000000","0.000000"], +"fcalphase":["-0.011757","0.020172"], +"fcaltilt":["-0.003302","-0.001370"], +"fcalcurve":["0.000323","-0.002600"], +"fcalgibpha":["-4.316406","0.740723"], +"fcalgibmag":["0.001188","-0.009270"] +} +"lighthouse1":{ +"index":"-1" +} +``` + +To make libsurvive calibrate and run with one basestations, `LighthouseCount` needs to be changed to `1`. + +It may be annoying to always require the controllers for calibration. To make libsurvive calibrate by using the HMD, `RequiredTrackersForCal` needs to be changed to the magic string `HMD`. The strings for the controllers are `WM0` and `WM1`, short for "Watchman". Other possible values are `WW0` (Wired Watchman) for a controller directly connected with USB or `TR0` for a Vive tracker directly connected with USB (When connected wirelessly, the tracker uses the dongles, so uses `WM0` or `WM1`). + +Lastly, to ensure libsurvive calibrates using the HMD, `AllowAllTrackersForCal` can be changed to `0`. + +Here is an example for such an altered `config.json` file + +``` +"LighthouseCount":"1", +"DefaultPoser":"PoserTurveyTori", +"RequiredTrackersForCal":"HMD", +"AllowAllTrackersForCal":"0", +"ConfigPoser":"PoserTurveyTori", +"TurveyToriDebug":"0" +"lighthouse0":{ +"index":"0", +"id":"138441170", +"pose":["0.000000","0.000000","0.000000","0.000000","0.000000","0.000000","0.000000"], +"fcalphase":["-0.011757","0.020172"], +"fcaltilt":["-0.003302","-0.001370"], +"fcalcurve":["0.000323","-0.002600"], +"fcalgibpha":["-4.316406","0.740723"], +"fcalgibmag":["0.001188","-0.009270"] +} +"lighthouse1":{ +"index":"-1" +} +``` + +Running libsurvive's `./test` with this `config.json` in the same directory should now go through the calibration and start printing poses with only the HMD and only one lighthouse basestation active. Enabling and tracking controllers will still work with this configuration. + +**For best results the HMD should not be moved while calibrating!** + +The important calibration steps are denoted by libsurvive printing + +``` +Info: Stage 2 good - continuing. 32 1 0 +Info: Stage 2 good - continuing. 32 1 1 +Info: Stage 2 good - continuing. 32 1 2 +Info: Stage 2 good - continuing. 32 1 3 +Info: Stage 2 good - continuing. 32 1 4 +Info: Stage 2 moving to stage 3. 32 1 5 +Lighthouse Pose: [0][ 0.28407975, 0.93606335,-0.37406892] [ 0.05594964,-0.33792987, 0.93887696, 0.03439615] +Info: Stage 4 succeeded. +``` + +If libsurvive does not print these steps, make sure that the lighthouse basestation is visible to enough sensors on the HMD. + +Sometimes libsurvive goes very quickly through these steps and fills in all pose values as `NaN` or `-NaN`. This appears to be a bug in libsurvive that has not be found yet. Reflective surfaces nearby may trigger this problem more often. + +[Here is a short demo video how successfuly running ./test should look like](https://haagch.frickel.club/Peek%202018-02-21%2023-23.webm). + +## Using libsurvive in your own application + +Example code for libsurvive can be found in [test.c](https://github.com/cnlohr/libsurvive/blob/master/test.c). [calibrate.c](https://github.com/cnlohr/libsurvive/blob/master/calibrate.c) may contain some interesting code too. + +Here is minimal example that demonstrates using libsurvive's callback functionality to fill in pose data into a user defined data structure that is stored in libsurvive's SurviveContext. + +```c +#include +#include +#include + +typedef struct { + double rotation[4]; + double pos[3]; +} libsurvive_hmd; + +void testprog_raw_pose_process(SurviveObject *so, uint8_t lighthouse, FLT *pos, FLT *quat) { + survive_default_raw_pose_process(so, lighthouse, pos, quat); + printf("(Callback) Pose: [%1.1x][%s][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, so->codename, pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]); + if (strcmp(so->codename, "HMD") == 0 && lighthouse == 0) { + libsurvive_hmd *hmd = so->ctx->user_ptr; + hmd->pos[0] = pos[0]; hmd->pos[1] = pos[1]; hmd->pos[2] = pos[2]; + hmd->rotation[0] = quat[0]; hmd->rotation[1] = quat[1]; hmd->rotation[2] = quat[2]; hmd->rotation[3] = quat[3]; + } +} + +int main(int argc, char** argv) { + struct SurviveContext *ctx = survive_init( 0 ); + survive_install_raw_pose_fn(ctx, testprog_raw_pose_process); + survive_cal_install(ctx); + libsurvive_hmd *hmd = &(libsurvive_hmd) { 0 }; + ctx->user_ptr = hmd; + while(survive_poll(ctx) == 0) { + //printf("(Main) HMD Pose: [% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", hmd->pos[0], hmd->pos[1], hmd->pos[2], hmd->rotation[0], hmd->rotation[1], hmd->rotation[2], hmd->rotation[3]); + } + return 0; +} +``` + +Compiling this minimal example only requires the include path for survive.h as well as the libsurvive library: `gcc demo.c -Iinclude/libsurvive/ -Llib -lsurvive -Wl,-rpath=./lib -o libsurvive-demo`. + + As mentioned, only the pose from lighthouse number `0` is used. Since the callback is called for all tracked devices, `so->codename` can be used to differentiate between devices like `HMD`, `WM0`, etc. + +# FAQ + +* The tracking quality is bad/jitters/too slow! + * libsurvive is still a work in progress. For example the Vive contains a calibration blob that still needs to be decoded. Hopefully it will enable better tracking. +* What VR software can I use with libsurvive? + * There is an unofficial [OpenHMD/libsurvive fork](https://github.com/ChristophHaag/OpenHMD/tree/libsurvive) that replaces OpenHMD's Vive driver with libsurvive. OpenHMD will not merge this branch as it depends on libsurvive as an external dependency, but it may pave the way for more code sharing. + * This OpenHMD/libsurvive fork can be plugged into [SteamVR-OpenHMD](https://github.com/ChristophHaag/SteamVR-OpenHMD) which allows SteamVR to use OpenHMD drivers. + * Godot 3.x has a [native OpenHMD plugin](https://github.com/BastiaanOlij/godot_openhmd) though it needs work for building and running properly and it is still missing motion controller support. ## Addendum and notes -- cgit v1.2.3 From 48e32da8ec20f8f4df934e2de8ebe4385b483c51 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 4 Mar 2018 18:51:32 -0700 Subject: Fixed inadvertent change to signedness of acceldata --- src/ootx_decoder.c | 4 ++-- src/survive_vive.c | 13 +++++++++---- 2 files changed, 11 insertions(+), 6 deletions(-) diff --git a/src/ootx_decoder.c b/src/ootx_decoder.c index ad55f5b..b7327d5 100644 --- a/src/ootx_decoder.c +++ b/src/ootx_decoder.c @@ -183,12 +183,12 @@ union iFloat { }; -struct __attribute__((__packed__)) unaligned_16_t { +struct __attribute__((__packed__)) unaligned_u16_t { uint16_t v; }; float _half_to_float(uint8_t* data) { - uint16_t x = ((struct unaligned_16_t*)data)->v; + uint16_t x = ((struct unaligned_u16_t*)data)->v; union iFloat fnum; fnum.f = 0; diff --git a/src/survive_vive.c b/src/survive_vive.c index 5be2d03..cb05efc 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -903,15 +903,20 @@ int survive_get_config( char ** config, SurviveViveData * sv, int devno, int ifa #define POP1 (*(readdata++)) - struct __attribute__((__packed__)) unaligned_16_t { - uint16_t v; + int16_t v; }; struct __attribute__((__packed__)) unaligned_32_t { + int32_t v; +}; +struct __attribute__((__packed__)) unaligned_u16_t { + uint16_t v; +}; +struct __attribute__((__packed__)) unaligned_u32_t { uint32_t v; }; -#define POP2 ((((( struct unaligned_16_t*)((readdata+=2)-2))))->v) -#define POP4 ((((( struct unaligned_32_t*)((readdata+=4)-4))))->v) +#define POP2 ((((( struct unaligned_u16_t*)((readdata+=2)-2))))->v) +#define POP4 ((((( struct unaligned_u32_t*)((readdata+=4)-4))))->v) void calibrate_acc(SurviveObject* so, FLT* agm) { if (so->acc_bias != NULL) { -- cgit v1.2.3 From 728f6f9e7f0a99aa584e7f35ade387e852b5fa83 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Wed, 7 Mar 2018 22:41:10 -0700 Subject: Added a check at init that makes sure the user agrees with what FLT is --- include/libsurvive/survive.h | 10 +++++++++- src/survive.c | 9 ++++++++- 2 files changed, 17 insertions(+), 2 deletions(-) diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 30f5817..d9b5f08 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -156,7 +156,15 @@ struct SurviveContext }; -SurviveContext * survive_init( int headless ); +SurviveContext * survive_init_internal( int headless ); + +// Baked in size of FLT to verify users of the library have the correct setting. +void survive_verify_FLT_size(uint32_t user_size); + +static inline SurviveContext * survive_init( int headless ) { + survive_verify_FLT_size(sizeof(FLT)); + return survive_init_internal( headless ); +} //For any of these, you may pass in 0 for the function pointer to use default behavior. //In general unless you are doing wacky things like recording or playing back data, you won't need to use this. diff --git a/src/survive.c b/src/survive.c index 0386275..4e52637 100755 --- a/src/survive.c +++ b/src/survive.c @@ -94,7 +94,14 @@ static void *button_servicer(void * context) return NULL; } -SurviveContext * survive_init( int headless ) +void survive_verify_FLT_size(uint32_t user_size) { + if(sizeof(FLT) != user_size) { + fprintf(stderr, "FLT type incompatible; the shared library has FLT size %lu vs user program %u\n", sizeof(FLT), user_size); + exit(-1); + } +} + +SurviveContext * survive_init_internal( int headless ) { #ifdef RUNTIME_SYMNUM if( !did_runtime_symnum ) -- cgit v1.2.3 From f5022be0b94cbd5b5ead8dc6035551ae1d011db0 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Thu, 8 Mar 2018 11:25:12 -0700 Subject: Updated error message to be more helpful --- src/survive.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/survive.c b/src/survive.c index 4e52637..76bf8e4 100755 --- a/src/survive.c +++ b/src/survive.c @@ -96,7 +96,9 @@ static void *button_servicer(void * context) void survive_verify_FLT_size(uint32_t user_size) { if(sizeof(FLT) != user_size) { - fprintf(stderr, "FLT type incompatible; the shared library has FLT size %lu vs user program %u\n", sizeof(FLT), user_size); + fprintf(stderr, "FLT type incompatible; the shared library libsurvive has FLT size %lu vs user program %u\n", sizeof(FLT), user_size); + fprintf(stderr, "Add '#define FLT %s' before including survive.h or recompile the shared library with the appropriate flag. \n", + sizeof(FLT) == sizeof(double) ? "double" : "float"); exit(-1); } } -- cgit v1.2.3 From 5aa1b9214745a625ab644846ada3034e7c38f493 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Thu, 8 Mar 2018 15:08:14 -0700 Subject: Added playback device driver --- Makefile | 2 +- data_recorder.c | 118 +++++++----- include/libsurvive/survive_types.h | 6 + src/survive_default_devices.c | 46 +++++ src/survive_default_devices.h | 12 ++ src/survive_playback.c | 374 +++++++++++++++++++++++++++++++++++++ src/survive_vive.c | 101 +++------- 7 files changed, 539 insertions(+), 120 deletions(-) create mode 100644 src/survive_default_devices.c create mode 100644 src/survive_default_devices.h create mode 100755 src/survive_playback.c diff --git a/Makefile b/Makefile index 69b8e37..e57dae5 100644 --- a/Makefile +++ b/Makefile @@ -37,7 +37,7 @@ REDISTS:=redist/json_helpers.o redist/linmath.o redist/jsmn.o redist/os_generic. ifeq ($(UNAME), Darwin) REDISTS:=$(REDISTS) redist/hid-osx.c endif -LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_vive.o src/survive_config.o src/survive_cal.o +LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_default_devices.o src/survive_vive.o src/survive_playback.o src/survive_config.o src/survive_cal.o #If you want to use HIDAPI on Linux. diff --git a/data_recorder.c b/data_recorder.c index 002357e..323d208 100644 --- a/data_recorder.c +++ b/data_recorder.c @@ -3,6 +3,7 @@ #ifdef __linux__ #include #endif + #include #include #include @@ -10,9 +11,14 @@ #include #include #include +#include +#include +#include struct SurviveContext * ctx; +FILE* output_file = 0; + void HandleKey( int keycode, int bDown ) { if( !bDown ) return; @@ -43,56 +49,70 @@ int bufferpts[32*2*3]; char buffermts[32*128*3]; int buffertimeto[32*3]; +uint64_t timestamp_in_us() { + static uint64_t start_time_us = 0; + struct timeval tv; + gettimeofday(&tv,NULL); + uint64_t now = (uint64_t)tv.tv_sec * 1000000L + tv.tv_usec; + if(start_time_us == 0) + start_time_us = now; + return now - start_time_us; +} + +int write_to_output(const char *format, ...) +{ + va_list args; + va_start(args, format); + fprintf(output_file, "%lu ", timestamp_in_us()); + vfprintf(output_file, format, args); + + va_end(args); +} + void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh) { survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length, lh); - if( acode == -1 ) return; -//return; + if( acode == -1 ) { + write_to_output( "A %s %d %d %d %u %u %u\n", so->codename, sensor_id, acode, timeinsweep, timecode, length, lh ); + return; + } + int jumpoffset = sensor_id; if( strcmp( so->codename, "WM0" ) == 0 ) jumpoffset += 32; else if( strcmp( so->codename, "WM1" ) == 0 ) jumpoffset += 64; - - if( acode == 0 || acode == 2 ) //data = 0 - { - printf( "L X %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length ); - bufferpts[jumpoffset*2+0] = (timeinsweep-100000)/500; - buffertimeto[jumpoffset] = 0; - } - if( acode == 1 || acode == 3 ) //data = 1 - { - printf( "L Y %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length ); - bufferpts[jumpoffset*2+1] = (timeinsweep-100000)/500; - buffertimeto[jumpoffset] = 0; - } - - - if( acode == 4 || acode == 6 ) //data = 0 - { - printf( "R X %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length ); - bufferpts[jumpoffset*2+0] = (timeinsweep-100000)/500; - buffertimeto[jumpoffset] = 0; - } - if( acode == 5 || acode == 7 ) //data = 1 - { - printf( "R Y %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length ); - bufferpts[jumpoffset*2+1] = (timeinsweep-100000)/500; - buffertimeto[jumpoffset] = 0; - } + const char* LH_ID = 0; + const char* LH_Axis = 0; + + switch(acode) { + case 0: + case 2: + bufferpts[jumpoffset*2+0] = (timeinsweep-100000)/500; + LH_ID = "L"; LH_Axis = "X"; break; + case 1: + case 3: + bufferpts[jumpoffset*2+1] = (timeinsweep-100000)/500; + LH_ID = "L"; LH_Axis = "Y"; break; + case 4: + case 6: + bufferpts[jumpoffset*2+0] = (timeinsweep-100000)/500; + LH_ID = "R"; LH_Axis = "X"; break; + case 5: + case 7: + bufferpts[jumpoffset*2+1] = (timeinsweep-100000)/500; + LH_ID = "R"; LH_Axis = "Y"; break; + } + + write_to_output( "%s %s %s %u %d %d %d %u %u\n", LH_ID, LH_Axis, so->codename, timecode, sensor_id, acode, timeinsweep, length, lh ); + buffertimeto[jumpoffset] = 0; } void my_imu_process( struct SurviveObject * so, int mask, FLT * accelgyro, uint32_t timecode, int id ) { survive_default_imu_process( so, mask, accelgyro, timecode, id ); - -//return; - //if( so->codename[0] == 'H' ) - if( 1 ) - { - printf( "I %s %d %f %f %f %f %f %f %d\n", so->codename, timecode, accelgyro[0], accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id ); - } + write_to_output( "I %s %d %u %.17g %.17g %.17g %.17g %.17g %.17g %d\n", so->codename, mask, timecode, accelgyro[0], accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id ); } @@ -156,22 +176,30 @@ void *SurviveThread(void *junk) while(survive_poll(ctx) == 0) { - printf("Do stuff.\n"); - //Do stuff. } return 0; } -int main() +int main(int argc, char** argv) { - // Create the libsurvive thread - OGCreateThread(SurviveThread, 0); + if(argc > 1) { + output_file = fopen(argv[1], "w"); + if(output_file == 0) { + fprintf(stderr, "Could not open %s for writing", argv[1]); + return -1; + } + } else { + output_file = stdout; + } + + // Create the libsurvive thread + OGCreateThread(SurviveThread, 0); - // Wait for the survive thread to load - while (!SurviveThreadLoaded) { OGUSleep(100); } + // Wait for the survive thread to load + while (!SurviveThreadLoaded) { OGUSleep(100); } - // Run the Gui in the main thread - GuiThread(0); + // Run the Gui in the main thread + GuiThread(0); } diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h index 5384345..be1115b 100644 --- a/include/libsurvive/survive_types.h +++ b/include/libsurvive/survive_types.h @@ -14,6 +14,12 @@ extern "C" { #endif #endif +#define float_format "%f" +#define double_format "%lf" +#define _FLT_format2(f) f##_format +#define _FLT_format(f) _FLT_format2(f) +#define FLT_format _FLT_format(FLT) + typedef struct SurvivePose { FLT Pos[3]; diff --git a/src/survive_default_devices.c b/src/survive_default_devices.c new file mode 100644 index 0000000..5d2cda7 --- /dev/null +++ b/src/survive_default_devices.c @@ -0,0 +1,46 @@ +#include +#include +#include "survive_default_devices.h" + +static SurviveObject* survive_create_device(SurviveContext * ctx, + const char* driver_name, + void* driver, + const char* device_name, + haptic_func fn) { + SurviveObject * device = calloc( 1, sizeof( SurviveObject ) ); + + device->ctx = ctx; + device->driver = driver; + memcpy( device->codename, device_name, strlen(device_name) ); + memcpy( device->drivername, driver_name, strlen(driver_name) ); + + device->timebase_hz = 48000000; + device->pulsedist_max_ticks = 500000; + device->pulselength_min_sync = 2200; + device->pulse_in_clear_time = 35000; + device->pulse_max_for_sweep = 1800; + device->pulse_synctime_offset = 20000; + device->pulse_synctime_slack = 5000; + device->timecenter_ticks = device->timebase_hz / 240; + + device->haptic = fn; + + return device; +} + +SurviveObject* survive_create_hmd(SurviveContext * ctx, const char* driver_name, void* driver) { + return survive_create_device(ctx, driver_name, driver, "HMD", 0); +} + +SurviveObject* survive_create_wm0(SurviveContext * ctx, const char* driver_name, void* driver, haptic_func fn) { + return survive_create_device(ctx, driver_name, driver, "WM0", fn); +} +SurviveObject* survive_create_wm1(SurviveContext * ctx, const char* driver_name, void* driver, haptic_func fn) { + return survive_create_device(ctx, driver_name, driver, "WM1", fn); +} +SurviveObject* survive_create_tr0(SurviveContext * ctx, const char* driver_name, void* driver) { + return survive_create_device(ctx, driver_name, driver, "TR0", 0); +} +SurviveObject* survive_create_ww0(SurviveContext * ctx, const char* driver_name, void* driver) { + return survive_create_device(ctx, driver_name, driver, "WW0", 0); +} diff --git a/src/survive_default_devices.h b/src/survive_default_devices.h new file mode 100644 index 0000000..c2ebc0b --- /dev/null +++ b/src/survive_default_devices.h @@ -0,0 +1,12 @@ +#ifndef _SURVIVE_DEFAULT_DEVICES_H +#define _SURVIVE_DEFAULT_DEVICES_H + +#include + +SurviveObject* survive_create_hmd(SurviveContext * ctx, const char* driver_name, void* driver); +SurviveObject* survive_create_wm0(SurviveContext * ctx, const char* driver_name, void* driver, haptic_func cb); +SurviveObject* survive_create_wm1(SurviveContext * ctx, const char* driver_name, void* driver, haptic_func cb); +SurviveObject* survive_create_tr0(SurviveContext * ctx, const char* driver_name, void* driver); +SurviveObject* survive_create_ww0(SurviveContext * ctx, const char* driver_name, void* driver); + +#endif diff --git a/src/survive_playback.c b/src/survive_playback.c new file mode 100755 index 0000000..bb66f25 --- /dev/null +++ b/src/survive_playback.c @@ -0,0 +1,374 @@ +//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 +#include +#include +#include +#include +#include +#include +#include +#if !defined(__FreeBSD__) && !defined(__APPLE__) +#include // for alloca +#endif +#include +#include "json_helpers.h" +#include "survive_config.h" +#include "survive_default_devices.h" + +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( SurviveContext * ctx, 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]; + + 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; +} + +struct SurvivePlaybackData { + SurviveContext * ctx; + const char* playback_dir; + FILE* playback_file; + int lineno; + + uint64_t next_time_us; +}; +typedef struct SurvivePlaybackData SurvivePlaybackData; + + +uint64_t timestamp_in_us() { + static uint64_t start_time_us = 0; + struct timeval tv; + gettimeofday(&tv,NULL); + uint64_t now = (uint64_t)tv.tv_sec * 1000000L + tv.tv_usec; + if(start_time_us == 0) + start_time_us = now; + return now - start_time_us; +} + +static int parse_and_run_imu(const char* line, SurvivePlaybackData* driver) { + char dev[10]; + int timecode = 0; + FLT accelgyro[6]; + int mask; + int id; + + int rr = sscanf(line,"I %s %d %d " FLT_format " " FLT_format " " FLT_format " " FLT_format " " FLT_format " " FLT_format "%d", dev, + &mask, + &timecode, + &accelgyro[0], &accelgyro[1], &accelgyro[2], + &accelgyro[3], &accelgyro[4], &accelgyro[5], &id ); + + if( rr != 10 ) + { + fprintf( stderr, "Warning: On line %d, only %d values read: '%s'\n", driver->lineno, rr, line ); + return -1; + } + + SurviveObject * so = survive_get_so_by_name( driver->ctx, dev); + if(!so) { + fprintf(stderr, "Could not find device named %s from lineno %d\n", dev, driver->lineno); + return -1; + } + + driver->ctx->imuproc( so, mask, accelgyro, timecode, id); + return 0; +} + + +static int parse_and_run_lightcode(const char* line, SurvivePlaybackData* driver) { + char lhn[10]; + char axn[10]; + char dev[10]; + uint32_t timecode = 0; + int sensor = 0; + int acode = 0; + int timeinsweep = 0; + uint32_t pulselength = 0; + uint32_t lh = 0; + + int rr = sscanf(line,"%8s %8s %8s %u %d %d %d %u %u\n", + lhn, axn, dev, + &timecode, &sensor, &acode, + &timeinsweep, &pulselength, &lh ); + + if( rr != 9 ) + { + fprintf( stderr, "Warning: On line %d, only %d values read: '%s'\n", driver->lineno, rr, line ); + return -1; + } + + SurviveObject * so = survive_get_so_by_name( driver->ctx, dev); + if(!so) { + fprintf(stderr, "Could not find device named %s from lineno %d\n", dev, driver->lineno); + return -1; + } + + driver->ctx->lightproc( so, sensor, acode, timeinsweep, timecode, pulselength, lh); + return 0; +} + +static int playback_poll( struct SurviveContext * ctx, void * _driver ) { + SurvivePlaybackData* driver = _driver; + FILE* f = driver->playback_file; + + if(f && !feof(f) && !ferror(f) ) + { + int i; + driver->lineno++; + char * line; + + if(driver->next_time_us == 0) { + char * buffer; + size_t n = 0; + ssize_t r = getdelim( &line, &n, ' ', f ); + if( r <= 0 ) return 0; + + uint64_t timestamp; + if(sscanf(line, "%lu", &driver->next_time_us) != 1) { + free(line); + return 0; + } + free(line); + line = 0; + } + + if(driver->next_time_us > timestamp_in_us()) + return 0; + driver->next_time_us = 0; + + char * buffer; + size_t n = 0; + ssize_t r = getline( &line, &n, f ); + if( r <= 0 ) return 0; + + if((line[0] != 'R' && line[0] != 'L' && line[0] != 'I') || line[1] != ' ' ) + return 0; + + switch(line[0]) { + case 'L': + case 'R': + parse_and_run_lightcode(line, driver); + break; + case 'I': + parse_and_run_imu(line, driver); + break; + } + + free( line ); + } else { + if(f) { + fclose(driver->playback_file); + } + driver->playback_file = 0; + return -1; + } + + return 0; +} + +int playback_close( struct SurviveContext * ctx, void * _driver ) { + SurvivePlaybackData* driver = _driver; + if(driver->playback_file) + fclose(driver->playback_file); + driver->playback_file = 0; + return 0; +} + + +static int LoadConfig( SurvivePlaybackData * sv, SurviveObject * so, int devno, int iface, int extra_magic ) +{ + SurviveContext * ctx = sv->ctx; + char * ct0conf = 0; + + char fname[100]; + sprintf( fname, "%s/%s_config.json", sv->playback_dir, so->codename ); + FILE * f = fopen( fname, "r" ); + + if(f == 0 || feof(f) || ferror(f) ) + return 1; + + fseek(f, 0, SEEK_END); + int len = ftell(f); + fseek(f, 0, SEEK_SET); //same as rewind(f); + + ct0conf = malloc(len+1); + fread( ct0conf, len, 1, f); + fclose( f ); + ct0conf[len] = 0; + + printf( "Loading config: %d\n", len ); + + if (len == 0) + return 1; + + //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; + } + } + + + if (jsoneq(ct0conf, tk, "acc_bias") == 0) { + int32_t count = (tk+1)->size; + FLT* values = NULL; + if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { + so->acc_bias = values; + so->acc_bias[0] *= .125; //XXX Wat? Observed by CNL. Biasing by more than this seems to hose things. + so->acc_bias[1] *= .125; + so->acc_bias[2] *= .125; + } + } + if (jsoneq(ct0conf, tk, "acc_scale") == 0) { + int32_t count = (tk+1)->size; + FLT* values = NULL; + if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { + so->acc_scale = values; + } + } + + if (jsoneq(ct0conf, tk, "gyro_bias") == 0) { + int32_t count = (tk+1)->size; + FLT* values = NULL; + if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { + so->gyro_bias = values; + } + } + if (jsoneq(ct0conf, tk, "gyro_scale") == 0) { + int32_t count = (tk+1)->size; + FLT* values = NULL; + if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { + so->gyro_scale = values; + } + } + } + + return 0; +} + + + +int DriverRegPlayback( SurviveContext * ctx ) +{ + const char* playback_dir = config_read_str(ctx->global_config_values, + "PlaybackDir", ""); + + if(strlen(playback_dir) == 0) { + return 0; +} + + SurvivePlaybackData * sp = calloc( 1, sizeof( SurvivePlaybackData ) ); + sp->ctx = ctx; + sp->playback_dir = playback_dir; + printf("%s\n", playback_dir); + + char playback_file[100]; + sprintf( playback_file, "%s/events", playback_dir ); + sp->playback_file = fopen( playback_file, "r"); + if(sp->playback_file == 0) { + fprintf(stderr, "Could not open playback events file %s", playback_file); + return -1; +} + SurviveObject * hmd = survive_create_hmd(ctx, "Playback", sp); + SurviveObject * wm0 = survive_create_wm0(ctx, "Playback", sp, 0); + SurviveObject * wm1 = survive_create_wm1(ctx, "Playback", sp, 0); + SurviveObject * tr0 = survive_create_tr0(ctx, "Playback", sp); + SurviveObject * ww0 = survive_create_ww0(ctx, "Playback", sp); + + if( !LoadConfig( sp, hmd, 1, 0, 0 )) { survive_add_object( ctx, hmd ); } + if( !LoadConfig( sp, wm0, 2, 0, 1 )) { survive_add_object( ctx, wm0 ); } + if( !LoadConfig( sp, wm1, 3, 0, 1 )) { survive_add_object( ctx, wm1 ); } + if( !LoadConfig( sp, tr0, 4, 0, 0 )) { survive_add_object( ctx, tr0 ); } + if( !LoadConfig( sp, ww0, 5, 0, 0 )) { survive_add_object( ctx, ww0 ); } + + + survive_add_driver(ctx, sp, playback_poll, playback_close, 0); + return 0; + fail_gracefully: + return -1; +} + +REGISTER_LINKTIME( DriverRegPlayback ); + diff --git a/src/survive_vive.c b/src/survive_vive.c index cb05efc..e21d419 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -23,6 +23,7 @@ #endif #include "json_helpers.h" +#include "survive_default_devices.h" #ifdef HIDAPI #if defined(WINDOWS) || defined(WIN32) || defined (_WIN32) @@ -1699,15 +1700,16 @@ static int LoadConfig( SurviveViveData * sv, SurviveObject * so, int devno, int SurviveContext * ctx = sv->ctx; char * ct0conf = 0; int len = survive_get_config( &ct0conf, sv, devno, iface, extra_magic ); -printf( "Loading config: %d\n", len ); -#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 + printf( "Loading config: %d\n", len ); + { + char raw_fname[100]; + sprintf( raw_fname, "%s_config.json", so->codename ); + FILE * f = fopen( raw_fname, "w" ); + fwrite( ct0conf, strlen(ct0conf), 1, f ); + fclose( f ); + } + if( len > 0 ) { @@ -1836,18 +1838,13 @@ void init_SurviveObject(SurviveObject* so) { int DriverRegHTCVive( SurviveContext * ctx ) { int r; - SurviveObject * hmd = calloc( 1, sizeof( SurviveObject ) ); - SurviveObject * wm0 = calloc( 1, sizeof( SurviveObject ) ); - SurviveObject * wm1 = calloc( 1, sizeof( SurviveObject ) ); - SurviveObject * tr0 = calloc( 1, sizeof( SurviveObject ) ); - SurviveObject * ww0 = calloc( 1, sizeof( SurviveObject ) ); - SurviveViveData * sv = calloc( 1, sizeof( SurviveViveData ) ); - - init_SurviveObject(hmd); - init_SurviveObject(wm0); - init_SurviveObject(wm1); - init_SurviveObject(tr0); - init_SurviveObject(ww0); + + SurviveViveData * sv = calloc(1, sizeof(SurviveViveData) ); + SurviveObject * hmd = survive_create_hmd(ctx, "HTC", sv); + SurviveObject * wm0 = survive_create_wm0(ctx, "HTC", sv, 0); + SurviveObject * wm1 = survive_create_wm1(ctx, "HTC", sv, 0); + SurviveObject * tr0 = survive_create_tr0(ctx, "HTC", sv); + SurviveObject * ww0 = survive_create_ww0(ctx, "HTC", sv); sv->ctx = ctx; @@ -1859,28 +1856,6 @@ int DriverRegHTCVive( SurviveContext * ctx ) mkdir( "calinfo", 0755 ); #endif - - hmd->ctx = ctx; - hmd->driver = sv; - memcpy( hmd->codename, "HMD", 4 ); - memcpy( hmd->drivername, "HTC", 4 ); - wm0->ctx = ctx; - wm0->driver = sv; - memcpy( wm0->codename, "WM0", 4 ); - memcpy( wm0->drivername, "HTC", 4 ); - wm1->ctx = ctx; - wm1->driver = sv; - memcpy( wm1->codename, "WM1", 4 ); - memcpy( wm1->drivername, "HTC", 4 ); - tr0->ctx = ctx; - tr0->driver = sv; - memcpy( tr0->codename, "TR0", 4 ); - memcpy( tr0->drivername, "HTC", 4 ); - ww0->ctx = ctx; - ww0->driver = sv; - memcpy( ww0->codename, "WW0", 4 ); - memcpy( ww0->drivername, "HTC", 4 ); - //USB must happen last. if( r = survive_usb_init( sv, hmd, wm0, wm1, tr0, ww0) ) { @@ -1894,36 +1869,6 @@ int DriverRegHTCVive( SurviveContext * ctx ) if( sv->udev[USB_DEV_WATCHMAN2] && LoadConfig( sv, wm1, 3, 0, 1 )) { SV_INFO( "Watchman 1 config issue." ); } if( sv->udev[USB_DEV_TRACKER0] && LoadConfig( sv, tr0, 4, 0, 0 )) { SV_INFO( "Tracker 0 config issue." ); } if( sv->udev[USB_DEV_W_WATCHMAN1] && LoadConfig( sv, ww0, 5, 0, 0 )) { SV_INFO( "Wired Watchman 0 config issue." ); } - - hmd->timebase_hz = wm0->timebase_hz = wm1->timebase_hz = 48000000; - tr0->timebase_hz = ww0->timebase_hz = hmd->timebase_hz; - - hmd->pulsedist_max_ticks = wm0->pulsedist_max_ticks = wm1->pulsedist_max_ticks = 500000; - tr0->pulsedist_max_ticks = ww0->pulsedist_max_ticks = hmd->pulsedist_max_ticks; - - hmd->pulselength_min_sync = wm0->pulselength_min_sync = wm1->pulselength_min_sync = 2200; - tr0->pulselength_min_sync = ww0->pulselength_min_sync = hmd->pulselength_min_sync; - - hmd->pulse_in_clear_time = wm0->pulse_in_clear_time = wm1->pulse_in_clear_time = 35000; - tr0->pulse_in_clear_time = ww0->pulse_in_clear_time = hmd->pulse_in_clear_time; - - hmd->pulse_max_for_sweep = wm0->pulse_max_for_sweep = wm1->pulse_max_for_sweep = 1800; - tr0->pulse_max_for_sweep = ww0->pulse_max_for_sweep = hmd->pulse_max_for_sweep; - - hmd->pulse_synctime_offset = wm0->pulse_synctime_offset = wm1->pulse_synctime_offset = 20000; - tr0->pulse_synctime_offset = ww0->pulse_synctime_offset = hmd->pulse_synctime_offset; - - hmd->pulse_synctime_slack = wm0->pulse_synctime_slack = wm1->pulse_synctime_slack = 5000; - tr0->pulse_synctime_slack = ww0->pulse_synctime_slack = hmd->pulse_synctime_slack; - - hmd->timecenter_ticks = hmd->timebase_hz / 240; - wm0->timecenter_ticks = wm0->timebase_hz / 240; - wm1->timecenter_ticks = wm1->timebase_hz / 240; - tr0->timecenter_ticks = tr0->timebase_hz / 240; - ww0->timecenter_ticks = ww0->timebase_hz / 240; - - wm0->haptic = survive_vive_send_haptic; - wm1->haptic = survive_vive_send_haptic; /* int i; int locs = hmd->nr_locations; @@ -1945,7 +1890,7 @@ int DriverRegHTCVive( SurviveContext * ctx ) } } */ - + //Add the drivers. if( sv->udev[USB_DEV_HMD_IMU_LH] ) { survive_add_object( ctx, hmd ); } if( sv->udev[USB_DEV_WATCHMAN1] ) { survive_add_object( ctx, wm0 ); } @@ -1953,7 +1898,15 @@ int DriverRegHTCVive( SurviveContext * ctx ) if( sv->udev[USB_DEV_TRACKER0] ) { survive_add_object( ctx, tr0 ); } if( sv->udev[USB_DEV_W_WATCHMAN1] ) { survive_add_object( ctx, ww0 ); } - survive_add_driver( ctx, sv, survive_vive_usb_poll, survive_vive_close, survive_vive_send_magic ); + if( sv->udev[USB_DEV_HMD_IMU_LH] || + sv->udev[USB_DEV_WATCHMAN1] || + sv->udev[USB_DEV_WATCHMAN2] || + sv->udev[USB_DEV_TRACKER0] || + sv->udev[USB_DEV_W_WATCHMAN1] ) { + survive_add_driver( ctx, sv, survive_vive_usb_poll, survive_vive_close, survive_vive_send_magic ); + } else { + fprintf(stderr, "No USB devices detected\n"); + } return 0; fail_gracefully: -- cgit v1.2.3 From a90f380359b719befb96af624260057848e95dce Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Thu, 8 Mar 2018 15:30:19 -0700 Subject: Refactoring to reuse code standard to htc json format --- src/survive_default_devices.c | 154 +++++++++++++++++++++++++++++++++++++ src/survive_default_devices.h | 1 + src/survive_internal.h | 1 + src/survive_playback.c | 154 +++---------------------------------- src/survive_vive.c | 173 +----------------------------------------- 5 files changed, 168 insertions(+), 315 deletions(-) diff --git a/src/survive_default_devices.c b/src/survive_default_devices.c index 5d2cda7..1882002 100644 --- a/src/survive_default_devices.c +++ b/src/survive_default_devices.c @@ -1,6 +1,10 @@ +#include #include #include #include "survive_default_devices.h" +#include + +#include "json_helpers.h" static SurviveObject* survive_create_device(SurviveContext * ctx, const char* driver_name, @@ -44,3 +48,153 @@ SurviveObject* survive_create_tr0(SurviveContext * ctx, const char* driver_name, SurviveObject* survive_create_ww0(SurviveContext * ctx, const char* driver_name, void* driver) { return survive_create_device(ctx, driver_name, driver, "WW0", 0); } + + +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( SurviveContext * ctx, 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]; + + 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; +} + +int survive_load_htc_config_format(char* ct0conf, int len, SurviveObject * so) { + if (len == 0) + return -1; + + SurviveContext* ctx = so->ctx; + //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; + } + } + + + if (jsoneq(ct0conf, tk, "acc_bias") == 0) { + int32_t count = (tk+1)->size; + FLT* values = NULL; + if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { + so->acc_bias = values; + so->acc_bias[0] *= .125; //XXX Wat? Observed by CNL. Biasing by more than this seems to hose things. + so->acc_bias[1] *= .125; + so->acc_bias[2] *= .125; + } + } + if (jsoneq(ct0conf, tk, "acc_scale") == 0) { + int32_t count = (tk+1)->size; + FLT* values = NULL; + if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { + so->acc_scale = values; + } + } + + if (jsoneq(ct0conf, tk, "gyro_bias") == 0) { + int32_t count = (tk+1)->size; + FLT* values = NULL; + if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { + so->gyro_bias = values; + } + } + if (jsoneq(ct0conf, tk, "gyro_scale") == 0) { + int32_t count = (tk+1)->size; + FLT* values = NULL; + if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { + so->gyro_scale = values; + } + } + } + + + char fname[64]; + + 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; +} diff --git a/src/survive_default_devices.h b/src/survive_default_devices.h index c2ebc0b..fce199d 100644 --- a/src/survive_default_devices.h +++ b/src/survive_default_devices.h @@ -9,4 +9,5 @@ SurviveObject* survive_create_wm1(SurviveContext * ctx, const char* driver_name, SurviveObject* survive_create_tr0(SurviveContext * ctx, const char* driver_name, void* driver); SurviveObject* survive_create_ww0(SurviveContext * ctx, const char* driver_name, void* driver); +int survive_load_htc_config_format(char* ct0conf, int length, SurviveObject * so); #endif diff --git a/src/survive_internal.h b/src/survive_internal.h index e1a733d..86b119f 100644 --- a/src/survive_internal.h +++ b/src/survive_internal.h @@ -17,6 +17,7 @@ void * GetDriver( const char * name ); const char * GetDriverNameMatching( const char * prefix, int place ); void ListDrivers(); + #endif diff --git a/src/survive_playback.c b/src/survive_playback.c index bb66f25..426a2b9 100755 --- a/src/survive_playback.c +++ b/src/survive_playback.c @@ -11,68 +11,15 @@ //All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses. #include -#include #include #include -#include + #include -#include -#include -#if !defined(__FreeBSD__) && !defined(__APPLE__) -#include // for alloca -#endif #include -#include "json_helpers.h" + #include "survive_config.h" #include "survive_default_devices.h" -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( SurviveContext * ctx, 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]; - - 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; -} - struct SurvivePlaybackData { SurviveContext * ctx; const char* playback_dir; @@ -215,7 +162,7 @@ static int playback_poll( struct SurviveContext * ctx, void * _driver ) { return 0; } -int playback_close( struct SurviveContext * ctx, void * _driver ) { +static int playback_close( struct SurviveContext * ctx, void * _driver ) { SurvivePlaybackData* driver = _driver; if(driver->playback_file) fclose(driver->playback_file); @@ -224,7 +171,7 @@ int playback_close( struct SurviveContext * ctx, void * _driver ) { } -static int LoadConfig( SurvivePlaybackData * sv, SurviveObject * so, int devno, int iface, int extra_magic ) +static int LoadConfig( SurvivePlaybackData * sv, SurviveObject * so) { SurviveContext * ctx = sv->ctx; char * ct0conf = 0; @@ -241,91 +188,12 @@ static int LoadConfig( SurvivePlaybackData * sv, SurviveObject * so, int devno, fseek(f, 0, SEEK_SET); //same as rewind(f); ct0conf = malloc(len+1); - fread( ct0conf, len, 1, f); + int read = fread( ct0conf, len, 1, f); fclose( f ); ct0conf[len] = 0; printf( "Loading config: %d\n", len ); - - if (len == 0) - return 1; - - //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; - } - } - - - if (jsoneq(ct0conf, tk, "acc_bias") == 0) { - int32_t count = (tk+1)->size; - FLT* values = NULL; - if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { - so->acc_bias = values; - so->acc_bias[0] *= .125; //XXX Wat? Observed by CNL. Biasing by more than this seems to hose things. - so->acc_bias[1] *= .125; - so->acc_bias[2] *= .125; - } - } - if (jsoneq(ct0conf, tk, "acc_scale") == 0) { - int32_t count = (tk+1)->size; - FLT* values = NULL; - if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { - so->acc_scale = values; - } - } - - if (jsoneq(ct0conf, tk, "gyro_bias") == 0) { - int32_t count = (tk+1)->size; - FLT* values = NULL; - if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { - so->gyro_bias = values; - } - } - if (jsoneq(ct0conf, tk, "gyro_scale") == 0) { - int32_t count = (tk+1)->size; - FLT* values = NULL; - if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { - so->gyro_scale = values; - } - } - } - - return 0; + return survive_load_htc_config_format(ct0conf, len, so); } @@ -357,11 +225,11 @@ int DriverRegPlayback( SurviveContext * ctx ) SurviveObject * tr0 = survive_create_tr0(ctx, "Playback", sp); SurviveObject * ww0 = survive_create_ww0(ctx, "Playback", sp); - if( !LoadConfig( sp, hmd, 1, 0, 0 )) { survive_add_object( ctx, hmd ); } - if( !LoadConfig( sp, wm0, 2, 0, 1 )) { survive_add_object( ctx, wm0 ); } - if( !LoadConfig( sp, wm1, 3, 0, 1 )) { survive_add_object( ctx, wm1 ); } - if( !LoadConfig( sp, tr0, 4, 0, 0 )) { survive_add_object( ctx, tr0 ); } - if( !LoadConfig( sp, ww0, 5, 0, 0 )) { survive_add_object( ctx, ww0 ); } + if( !LoadConfig( sp, hmd )) { survive_add_object( ctx, hmd ); } + if( !LoadConfig( sp, wm0 )) { survive_add_object( ctx, wm0 ); } + if( !LoadConfig( sp, wm1 )) { survive_add_object( ctx, wm1 ); } + if( !LoadConfig( sp, tr0 )) { survive_add_object( ctx, tr0 ); } + if( !LoadConfig( sp, ww0 )) { survive_add_object( ctx, ww0 ); } survive_add_driver(ctx, sp, playback_poll, playback_close, 0); diff --git a/src/survive_vive.c b/src/survive_vive.c index e21d419..2472992 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -1648,53 +1648,6 @@ void survive_data_cb( SurviveUSBInterface * si ) -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( SurviveContext * ctx, 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]; - - 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( SurviveViveData * sv, SurviveObject * so, int devno, int iface, int extra_magic ) { SurviveContext * ctx = sv->ctx; @@ -1709,111 +1662,8 @@ static int LoadConfig( SurviveViveData * sv, SurviveObject * so, int devno, int fwrite( ct0conf, strlen(ct0conf), 1, f ); fclose( f ); } - - 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; - } - } - - - if (jsoneq(ct0conf, tk, "acc_bias") == 0) { - int32_t count = (tk+1)->size; - FLT* values = NULL; - if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { - so->acc_bias = values; - so->acc_bias[0] *= .125; //XXX Wat? Observed by CNL. Biasing by more than this seems to hose things. - so->acc_bias[1] *= .125; - so->acc_bias[2] *= .125; - } - } - if (jsoneq(ct0conf, tk, "acc_scale") == 0) { - int32_t count = (tk+1)->size; - FLT* values = NULL; - if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { - so->acc_scale = values; - } - } - - if (jsoneq(ct0conf, tk, "gyro_bias") == 0) { - int32_t count = (tk+1)->size; - FLT* values = NULL; - if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { - so->gyro_bias = values; - } - } - if (jsoneq(ct0conf, tk, "gyro_scale") == 0) { - int32_t count = (tk+1)->size; - FLT* values = NULL; - if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { - so->gyro_scale = values; - } - } - } - } - else - { - //TODO: Cleanup any remaining USB stuff. - return 1; - } - - char fname[64]; - - 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; + return survive_load_htc_config_format(ct0conf, len, so); } @@ -1869,27 +1719,6 @@ int DriverRegHTCVive( SurviveContext * ctx ) if( sv->udev[USB_DEV_WATCHMAN2] && LoadConfig( sv, wm1, 3, 0, 1 )) { SV_INFO( "Watchman 1 config issue." ); } if( sv->udev[USB_DEV_TRACKER0] && LoadConfig( sv, tr0, 4, 0, 0 )) { SV_INFO( "Tracker 0 config issue." ); } if( sv->udev[USB_DEV_W_WATCHMAN1] && LoadConfig( sv, ww0, 5, 0, 0 )) { SV_INFO( "Wired Watchman 0 config issue." ); } -/* - 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. if( sv->udev[USB_DEV_HMD_IMU_LH] ) { survive_add_object( ctx, hmd ); } -- cgit v1.2.3 From 58de3587d9e4d620daa57268cc74092120b7caac Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Thu, 8 Mar 2018 16:13:40 -0700 Subject: Disable usb driver if playback is active --- src/survive_vive.c | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/survive_vive.c b/src/survive_vive.c index 2472992..288f8cb 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -24,6 +24,7 @@ #include "json_helpers.h" #include "survive_default_devices.h" +#include "survive_config.h" #ifdef HIDAPI #if defined(WINDOWS) || defined(WIN32) || defined (_WIN32) @@ -1687,8 +1688,14 @@ void init_SurviveObject(SurviveObject* so) { int DriverRegHTCVive( SurviveContext * ctx ) { - int r; - + const char* playback_dir = config_read_str(ctx->global_config_values, + "PlaybackDir", ""); + if(strlen(playback_dir) != 0) { + SV_INFO("Playback is active; disabling USB driver"); + return 0; + } + + int r; SurviveViveData * sv = calloc(1, sizeof(SurviveViveData) ); SurviveObject * hmd = survive_create_hmd(ctx, "HTC", sv); SurviveObject * wm0 = survive_create_wm0(ctx, "HTC", sv, 0); -- cgit v1.2.3 From 82dc5c4452e2a58667c337e967200cb7760c16cd Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Thu, 8 Mar 2018 16:19:05 -0700 Subject: Added time factor to playback --- src/survive_playback.c | 19 ++++++------------- 1 file changed, 6 insertions(+), 13 deletions(-) diff --git a/src/survive_playback.c b/src/survive_playback.c index 426a2b9..d8c6fba 100755 --- a/src/survive_playback.c +++ b/src/survive_playback.c @@ -1,13 +1,3 @@ -//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 @@ -26,6 +16,7 @@ struct SurvivePlaybackData { FILE* playback_file; int lineno; + FLT time_factor; uint64_t next_time_us; }; typedef struct SurvivePlaybackData SurvivePlaybackData; @@ -128,7 +119,7 @@ static int playback_poll( struct SurviveContext * ctx, void * _driver ) { line = 0; } - if(driver->next_time_us > timestamp_in_us()) + if(driver->next_time_us * driver->time_factor > timestamp_in_us()) return 0; driver->next_time_us = 0; @@ -204,12 +195,14 @@ int DriverRegPlayback( SurviveContext * ctx ) "PlaybackDir", ""); if(strlen(playback_dir) == 0) { - return 0; -} + return 0; + } SurvivePlaybackData * sp = calloc( 1, sizeof( SurvivePlaybackData ) ); sp->ctx = ctx; sp->playback_dir = playback_dir; + sp->time_factor = config_read_float(ctx->global_config_values, "PlaybackFactor", 1.); + printf("%s\n", playback_dir); char playback_file[100]; -- cgit v1.2.3 From 3ec96c9a81f0f770fac75a1bdfc16ea96df07d9b Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Fri, 9 Mar 2018 00:21:17 -0700 Subject: Swapped to OGGetAbsoluteTime for timestamp --- data_recorder.c | 15 +++++++-------- src/survive_playback.c | 20 +++++++++----------- 2 files changed, 16 insertions(+), 19 deletions(-) diff --git a/data_recorder.c b/data_recorder.c index 323d208..63746c3 100644 --- a/data_recorder.c +++ b/data_recorder.c @@ -15,6 +15,8 @@ #include #include +#include "redist/os_generic.h" + struct SurviveContext * ctx; FILE* output_file = 0; @@ -49,21 +51,18 @@ int bufferpts[32*2*3]; char buffermts[32*128*3]; int buffertimeto[32*3]; -uint64_t timestamp_in_us() { - static uint64_t start_time_us = 0; - struct timeval tv; - gettimeofday(&tv,NULL); - uint64_t now = (uint64_t)tv.tv_sec * 1000000L + tv.tv_usec; +double timestamp_in_us() { + static double start_time_us = 0; if(start_time_us == 0) - start_time_us = now; - return now - start_time_us; + start_time_us = OGGetAbsoluteTime(); + return OGGetAbsoluteTime() - start_time_us; } int write_to_output(const char *format, ...) { va_list args; va_start(args, format); - fprintf(output_file, "%lu ", timestamp_in_us()); + fprintf(output_file, "%.17g ", timestamp_in_us()); vfprintf(output_file, format, args); va_end(args); diff --git a/src/survive_playback.c b/src/survive_playback.c index d8c6fba..7c49f5b 100755 --- a/src/survive_playback.c +++ b/src/survive_playback.c @@ -10,6 +10,8 @@ #include "survive_config.h" #include "survive_default_devices.h" +#include "redist/os_generic.h" + struct SurvivePlaybackData { SurviveContext * ctx; const char* playback_dir; @@ -17,19 +19,16 @@ struct SurvivePlaybackData { int lineno; FLT time_factor; - uint64_t next_time_us; + double next_time_us; }; typedef struct SurvivePlaybackData SurvivePlaybackData; -uint64_t timestamp_in_us() { - static uint64_t start_time_us = 0; - struct timeval tv; - gettimeofday(&tv,NULL); - uint64_t now = (uint64_t)tv.tv_sec * 1000000L + tv.tv_usec; - if(start_time_us == 0) - start_time_us = now; - return now - start_time_us; +double timestamp_in_us() { + static double start_time_us = 0; + if(start_time_us == 0.) + start_time_us = OGGetAbsoluteTime(); + return OGGetAbsoluteTime() - start_time_us; } static int parse_and_run_imu(const char* line, SurvivePlaybackData* driver) { @@ -110,8 +109,7 @@ static int playback_poll( struct SurviveContext * ctx, void * _driver ) { ssize_t r = getdelim( &line, &n, ' ', f ); if( r <= 0 ) return 0; - uint64_t timestamp; - if(sscanf(line, "%lu", &driver->next_time_us) != 1) { + if(sscanf(line, "%lf", &driver->next_time_us) != 1) { free(line); return 0; } -- cgit v1.2.3 From fe88ef57cff4971664b252cb9def6a5ddcc4276d Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Fri, 9 Mar 2018 00:41:46 -0700 Subject: Spaces -> Tabs (code formatting change) --- data_recorder.c | 250 ++++++++++++++-------------- src/survive_default_devices.c | 309 +++++++++++++++++----------------- src/survive_default_devices.h | 18 +- src/survive_playback.c | 375 +++++++++++++++++++++--------------------- 4 files changed, 483 insertions(+), 469 deletions(-) mode change 100755 => 100644 src/survive_playback.c diff --git a/data_recorder.c b/data_recorder.c index 63746c3..6123d57 100644 --- a/data_recorder.c +++ b/data_recorder.c @@ -1,204 +1,204 @@ -//Data recorder mod with GUI showing light positions. +// Data recorder mod with GUI showing light positions. #ifdef __linux__ #include #endif +#include +#include +#include +#include #include #include -#include -#include #include -#include -#include -#include +#include #include -#include +#include #include "redist/os_generic.h" -struct SurviveContext * ctx; +struct SurviveContext *ctx; -FILE* output_file = 0; +FILE *output_file = 0; -void HandleKey( int keycode, int bDown ) -{ - if( !bDown ) return; +void HandleKey(int keycode, int bDown) { + if (!bDown) + return; - if( keycode == 'O' || keycode == 'o' ) - { - survive_send_magic(ctx,1,0,0); + if (keycode == 'O' || keycode == 'o') { + survive_send_magic(ctx, 1, 0, 0); } - if( keycode == 'F' || keycode == 'f' ) - { - survive_send_magic(ctx,0,0,0); + if (keycode == 'F' || keycode == 'f') { + survive_send_magic(ctx, 0, 0, 0); } } -void HandleButton( int x, int y, int button, int bDown ) -{ -} +void HandleButton(int x, int y, int button, int bDown) {} -void HandleMotion( int x, int y, int mask ) -{ -} +void HandleMotion(int x, int y, int mask) {} -void HandleDestroy() -{ -} +void HandleDestroy() {} -int bufferpts[32*2*3]; -char buffermts[32*128*3]; -int buffertimeto[32*3]; +int bufferpts[32 * 2 * 3]; +char buffermts[32 * 128 * 3]; +int buffertimeto[32 * 3]; double timestamp_in_us() { - static double start_time_us = 0; - if(start_time_us == 0) - start_time_us = OGGetAbsoluteTime(); - return OGGetAbsoluteTime() - start_time_us; + static double start_time_us = 0; + if (start_time_us == 0) + start_time_us = OGGetAbsoluteTime(); + return OGGetAbsoluteTime() - start_time_us; } -int write_to_output(const char *format, ...) -{ - va_list args; - va_start(args, format); - fprintf(output_file, "%.17g ", timestamp_in_us()); - vfprintf(output_file, format, args); +int write_to_output(const char *format, ...) { + va_list args; + va_start(args, format); + fprintf(output_file, "%.17g ", timestamp_in_us()); + vfprintf(output_file, format, args); - va_end(args); + va_end(args); } -void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh) -{ - survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length, lh); +void my_light_process(struct SurviveObject *so, int sensor_id, int acode, + int timeinsweep, uint32_t timecode, uint32_t length, + uint32_t lh) { + survive_default_light_process(so, sensor_id, acode, timeinsweep, timecode, + length, lh); - if( acode == -1 ) { - write_to_output( "A %s %d %d %d %u %u %u\n", so->codename, sensor_id, acode, timeinsweep, timecode, length, lh ); - return; + if (acode == -1) { + write_to_output("A %s %d %d %d %u %u %u\n", so->codename, sensor_id, + acode, timeinsweep, timecode, length, lh); + return; } int jumpoffset = sensor_id; - if( strcmp( so->codename, "WM0" ) == 0 ) jumpoffset += 32; - else if( strcmp( so->codename, "WM1" ) == 0 ) jumpoffset += 64; + if (strcmp(so->codename, "WM0") == 0) + jumpoffset += 32; + else if (strcmp(so->codename, "WM1") == 0) + jumpoffset += 64; + + const char *LH_ID = 0; + const char *LH_Axis = 0; - const char* LH_ID = 0; - const char* LH_Axis = 0; - - switch(acode) { + switch (acode) { case 0: case 2: - bufferpts[jumpoffset*2+0] = (timeinsweep-100000)/500; - LH_ID = "L"; LH_Axis = "X"; break; + bufferpts[jumpoffset * 2 + 0] = (timeinsweep - 100000) / 500; + LH_ID = "L"; + LH_Axis = "X"; + break; case 1: case 3: - bufferpts[jumpoffset*2+1] = (timeinsweep-100000)/500; - LH_ID = "L"; LH_Axis = "Y"; break; + bufferpts[jumpoffset * 2 + 1] = (timeinsweep - 100000) / 500; + LH_ID = "L"; + LH_Axis = "Y"; + break; case 4: case 6: - bufferpts[jumpoffset*2+0] = (timeinsweep-100000)/500; - LH_ID = "R"; LH_Axis = "X"; break; + bufferpts[jumpoffset * 2 + 0] = (timeinsweep - 100000) / 500; + LH_ID = "R"; + LH_Axis = "X"; + break; case 5: case 7: - bufferpts[jumpoffset*2+1] = (timeinsweep-100000)/500; - LH_ID = "R"; LH_Axis = "Y"; break; - } + bufferpts[jumpoffset * 2 + 1] = (timeinsweep - 100000) / 500; + LH_ID = "R"; + LH_Axis = "Y"; + break; + } - write_to_output( "%s %s %s %u %d %d %d %u %u\n", LH_ID, LH_Axis, so->codename, timecode, sensor_id, acode, timeinsweep, length, lh ); + write_to_output("%s %s %s %u %d %d %d %u %u\n", LH_ID, LH_Axis, + so->codename, timecode, sensor_id, acode, timeinsweep, + length, lh); buffertimeto[jumpoffset] = 0; - } -void my_imu_process( struct SurviveObject * so, int mask, FLT * accelgyro, uint32_t timecode, int id ) -{ - survive_default_imu_process( so, mask, accelgyro, timecode, id ); - write_to_output( "I %s %d %u %.17g %.17g %.17g %.17g %.17g %.17g %d\n", so->codename, mask, timecode, accelgyro[0], accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id ); +void my_imu_process(struct SurviveObject *so, int mask, FLT *accelgyro, + uint32_t timecode, int id) { + survive_default_imu_process(so, mask, accelgyro, timecode, id); + write_to_output("I %s %d %u %.17g %.17g %.17g %.17g %.17g %.17g %d\n", + so->codename, mask, timecode, accelgyro[0], accelgyro[1], + accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id); } - - - -void * GuiThread( void * v ) -{ +void *GuiThread(void *v) { CNFGBGColor = 0x000000; CNFGDialogColor = 0x444444; - CNFGSetup( "Survive GUI Debug", 640, 480 ); + CNFGSetup("Survive GUI Debug", 640, 480); short screenx, screeny; - while(1) - { + while (1) { CNFGHandleInput(); CNFGClearFrame(); - CNFGColor( 0xFFFFFF ); - CNFGGetDimensions( &screenx, &screeny ); + CNFGColor(0xFFFFFF); + CNFGGetDimensions(&screenx, &screeny); int i; - for( i = 0; i < 32*3; i++ ) - { - if( buffertimeto[i] < 50 ) - { + for (i = 0; i < 32 * 3; i++) { + if (buffertimeto[i] < 50) { uint32_t color = i * 3231349; uint8_t r = color & 0xff; - uint8_t g = (color>>8) & 0xff; - uint8_t b = (color>>16) & 0xff; - r = (r * (5-buffertimeto[i])) / 5 ; - g = (g * (5-buffertimeto[i])) / 5 ; - b = (b * (5-buffertimeto[i])) / 5 ; - CNFGColor( (b<<16) | (g<<8) | r ); - CNFGTackRectangle( bufferpts[i*2+0], bufferpts[i*2+1], bufferpts[i*2+0] + 5, bufferpts[i*2+1] + 5 ); - CNFGPenX = bufferpts[i*2+0]; CNFGPenY = bufferpts[i*2+1]; - CNFGDrawText( buffermts, 2 ); + uint8_t g = (color >> 8) & 0xff; + uint8_t b = (color >> 16) & 0xff; + r = (r * (5 - buffertimeto[i])) / 5; + g = (g * (5 - buffertimeto[i])) / 5; + b = (b * (5 - buffertimeto[i])) / 5; + CNFGColor((b << 16) | (g << 8) | r); + CNFGTackRectangle(bufferpts[i * 2 + 0], bufferpts[i * 2 + 1], + bufferpts[i * 2 + 0] + 5, + bufferpts[i * 2 + 1] + 5); + CNFGPenX = bufferpts[i * 2 + 0]; + CNFGPenY = bufferpts[i * 2 + 1]; + CNFGDrawText(buffermts, 2); buffertimeto[i]++; } } CNFGSwapBuffers(); - OGUSleep( 10000 ); + OGUSleep(10000); } } -int SurviveThreadLoaded=0; +int SurviveThreadLoaded = 0; -void *SurviveThread(void *junk) -{ - ctx = survive_init( 0 ); +void *SurviveThread(void *junk) { + ctx = survive_init(0); - survive_install_light_fn( ctx, my_light_process ); - survive_install_imu_fn( ctx, my_imu_process ); + survive_install_light_fn(ctx, my_light_process); + survive_install_imu_fn(ctx, my_imu_process); - if( !ctx ) - { - fprintf( stderr, "Fatal. Could not start\n" ); + if (!ctx) { + fprintf(stderr, "Fatal. Could not start\n"); exit(1); } SurviveThreadLoaded = 1; - while(survive_poll(ctx) == 0) - { + while (survive_poll(ctx) == 0) { } - return 0; + return 0; } -int main(int argc, char** argv) -{ - if(argc > 1) { - output_file = fopen(argv[1], "w"); - if(output_file == 0) { - fprintf(stderr, "Could not open %s for writing", argv[1]); - return -1; - } - } else { - output_file = stdout; - } - - // Create the libsurvive thread - OGCreateThread(SurviveThread, 0); - - // Wait for the survive thread to load - while (!SurviveThreadLoaded) { OGUSleep(100); } - - // Run the Gui in the main thread - GuiThread(0); -} +int main(int argc, char **argv) { + if (argc > 1) { + output_file = fopen(argv[1], "w"); + if (output_file == 0) { + fprintf(stderr, "Could not open %s for writing", argv[1]); + return -1; + } + } else { + output_file = stdout; + } + + // Create the libsurvive thread + OGCreateThread(SurviveThread, 0); + // Wait for the survive thread to load + while (!SurviveThreadLoaded) { + OGUSleep(100); + } + + // Run the Gui in the main thread + GuiThread(0); +} diff --git a/src/survive_default_devices.c b/src/survive_default_devices.c index 1882002..6615f1e 100644 --- a/src/survive_default_devices.c +++ b/src/survive_default_devices.c @@ -1,93 +1,92 @@ +#include "survive_default_devices.h" #include +#include #include #include -#include "survive_default_devices.h" -#include #include "json_helpers.h" -static SurviveObject* survive_create_device(SurviveContext * ctx, - const char* driver_name, - void* driver, - const char* device_name, - haptic_func fn) { - SurviveObject * device = calloc( 1, sizeof( SurviveObject ) ); - - device->ctx = ctx; - device->driver = driver; - memcpy( device->codename, device_name, strlen(device_name) ); - memcpy( device->drivername, driver_name, strlen(driver_name) ); - - device->timebase_hz = 48000000; - device->pulsedist_max_ticks = 500000; - device->pulselength_min_sync = 2200; - device->pulse_in_clear_time = 35000; - device->pulse_max_for_sweep = 1800; - device->pulse_synctime_offset = 20000; - device->pulse_synctime_slack = 5000; - device->timecenter_ticks = device->timebase_hz / 240; - - device->haptic = fn; - - return device; +static SurviveObject * +survive_create_device(SurviveContext *ctx, const char *driver_name, + void *driver, const char *device_name, haptic_func fn) { + SurviveObject *device = calloc(1, sizeof(SurviveObject)); + + device->ctx = ctx; + device->driver = driver; + memcpy(device->codename, device_name, strlen(device_name)); + memcpy(device->drivername, driver_name, strlen(driver_name)); + + device->timebase_hz = 48000000; + device->pulsedist_max_ticks = 500000; + device->pulselength_min_sync = 2200; + device->pulse_in_clear_time = 35000; + device->pulse_max_for_sweep = 1800; + device->pulse_synctime_offset = 20000; + device->pulse_synctime_slack = 5000; + device->timecenter_ticks = device->timebase_hz / 240; + + device->haptic = fn; + + return device; } -SurviveObject* survive_create_hmd(SurviveContext * ctx, const char* driver_name, void* driver) { - return survive_create_device(ctx, driver_name, driver, "HMD", 0); +SurviveObject *survive_create_hmd(SurviveContext *ctx, const char *driver_name, + void *driver) { + return survive_create_device(ctx, driver_name, driver, "HMD", 0); } -SurviveObject* survive_create_wm0(SurviveContext * ctx, const char* driver_name, void* driver, haptic_func fn) { - return survive_create_device(ctx, driver_name, driver, "WM0", fn); +SurviveObject *survive_create_wm0(SurviveContext *ctx, const char *driver_name, + void *driver, haptic_func fn) { + return survive_create_device(ctx, driver_name, driver, "WM0", fn); } -SurviveObject* survive_create_wm1(SurviveContext * ctx, const char* driver_name, void* driver, haptic_func fn) { - return survive_create_device(ctx, driver_name, driver, "WM1", fn); +SurviveObject *survive_create_wm1(SurviveContext *ctx, const char *driver_name, + void *driver, haptic_func fn) { + return survive_create_device(ctx, driver_name, driver, "WM1", fn); } -SurviveObject* survive_create_tr0(SurviveContext * ctx, const char* driver_name, void* driver) { - return survive_create_device(ctx, driver_name, driver, "TR0", 0); +SurviveObject *survive_create_tr0(SurviveContext *ctx, const char *driver_name, + void *driver) { + return survive_create_device(ctx, driver_name, driver, "TR0", 0); } -SurviveObject* survive_create_ww0(SurviveContext * ctx, const char* driver_name, void* driver) { - return survive_create_device(ctx, driver_name, driver, "WW0", 0); +SurviveObject *survive_create_ww0(SurviveContext *ctx, const char *driver_name, + void *driver) { + return survive_create_device(ctx, driver_name, driver, "WW0", 0); } - 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) { + 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( SurviveContext * ctx, SurviveObject * so, char * ct0conf, FLT ** floats_out, jsmntok_t * t, int i ) -{ +static int ParsePoints(SurviveContext *ctx, SurviveObject *so, char *ct0conf, + FLT **floats_out, jsmntok_t *t, int i) { int k; - int pts = t[i+1].size; - jsmntok_t * tk; + int pts = t[i + 1].size; + jsmntok_t *tk; so->nr_locations = 0; - *floats_out = malloc( sizeof( **floats_out ) * 32 * 3 ); + *floats_out = malloc(sizeof(**floats_out) * 32 * 3); - for( k = 0; k < pts; k++ ) - { - tk = &t[i+2+k*4]; + for (k = 0; k < pts; k++) { + tk = &t[i + 2 + k * 4]; int m; - for( m = 0; m < 3; 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" ); + if (tk->type != 4 || elemlen > sizeof(ctt) - 1) { + SV_ERROR("Parse error in JSON\n"); return 1; } - memcpy( ctt, ct0conf + tk->start, elemlen ); + memcpy(ctt, ct0conf + tk->start, elemlen); ctt[elemlen] = 0; - FLT f = atof( ctt ); - int id = so->nr_locations*3+m; + FLT f = atof(ctt); + int id = so->nr_locations * 3 + m; (*floats_out)[id] = f; } so->nr_locations++; @@ -95,106 +94,106 @@ static int ParsePoints( SurviveContext * ctx, SurviveObject * so, char * ct0conf return 0; } -int survive_load_htc_config_format(char* ct0conf, int len, SurviveObject * so) { - if (len == 0) - return -1; - - SurviveContext* ctx = so->ctx; - //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; +int survive_load_htc_config_format(char *ct0conf, int len, SurviveObject *so) { + if (len == 0) + return -1; + + SurviveContext *ctx = so->ctx; + // 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 (jsoneq(ct0conf, tk, "modelNormals") == 0) { - if( ParsePoints( ctx, so, ct0conf, &so->sensor_normals, t, i ) ) - { - break; + if (r < 1 || t[0].type != JSMN_OBJECT) { + SV_INFO("Object expected in HMD configuration\n"); + return -2; } - } - - - if (jsoneq(ct0conf, tk, "acc_bias") == 0) { - int32_t count = (tk+1)->size; - FLT* values = NULL; - if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { - so->acc_bias = values; - so->acc_bias[0] *= .125; //XXX Wat? Observed by CNL. Biasing by more than this seems to hose things. - so->acc_bias[1] *= .125; - so->acc_bias[2] *= .125; - } - } - if (jsoneq(ct0conf, tk, "acc_scale") == 0) { - int32_t count = (tk+1)->size; - FLT* values = NULL; - if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { - so->acc_scale = values; - } - } - - if (jsoneq(ct0conf, tk, "gyro_bias") == 0) { - int32_t count = (tk+1)->size; - FLT* values = NULL; - if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { - so->gyro_bias = values; - } - } - if (jsoneq(ct0conf, tk, "gyro_scale") == 0) { - int32_t count = (tk+1)->size; - FLT* values = NULL; - if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { - so->gyro_scale = values; - } - } - } - - - char fname[64]; - - 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; + + 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; + } + } + + if (jsoneq(ct0conf, tk, "acc_bias") == 0) { + int32_t count = (tk + 1)->size; + FLT *values = NULL; + if (parse_float_array(ct0conf, tk + 2, &values, count) > 0) { + so->acc_bias = values; + so->acc_bias[0] *= .125; // XXX Wat? Observed by CNL. Biasing + // by more than this seems to hose + // things. + so->acc_bias[1] *= .125; + so->acc_bias[2] *= .125; + } + } + if (jsoneq(ct0conf, tk, "acc_scale") == 0) { + int32_t count = (tk + 1)->size; + FLT *values = NULL; + if (parse_float_array(ct0conf, tk + 2, &values, count) > 0) { + so->acc_scale = values; + } + } + + if (jsoneq(ct0conf, tk, "gyro_bias") == 0) { + int32_t count = (tk + 1)->size; + FLT *values = NULL; + if (parse_float_array(ct0conf, tk + 2, &values, count) > 0) { + so->gyro_bias = values; + } + } + if (jsoneq(ct0conf, tk, "gyro_scale") == 0) { + int32_t count = (tk + 1)->size; + FLT *values = NULL; + if (parse_float_array(ct0conf, tk + 2, &values, count) > 0) { + so->gyro_scale = values; + } + } + } + + char fname[64]; + + 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; } diff --git a/src/survive_default_devices.h b/src/survive_default_devices.h index fce199d..1fcca72 100644 --- a/src/survive_default_devices.h +++ b/src/survive_default_devices.h @@ -3,11 +3,17 @@ #include -SurviveObject* survive_create_hmd(SurviveContext * ctx, const char* driver_name, void* driver); -SurviveObject* survive_create_wm0(SurviveContext * ctx, const char* driver_name, void* driver, haptic_func cb); -SurviveObject* survive_create_wm1(SurviveContext * ctx, const char* driver_name, void* driver, haptic_func cb); -SurviveObject* survive_create_tr0(SurviveContext * ctx, const char* driver_name, void* driver); -SurviveObject* survive_create_ww0(SurviveContext * ctx, const char* driver_name, void* driver); +SurviveObject *survive_create_hmd(SurviveContext *ctx, const char *driver_name, + void *driver); +SurviveObject *survive_create_wm0(SurviveContext *ctx, const char *driver_name, + void *driver, haptic_func cb); +SurviveObject *survive_create_wm1(SurviveContext *ctx, const char *driver_name, + void *driver, haptic_func cb); +SurviveObject *survive_create_tr0(SurviveContext *ctx, const char *driver_name, + void *driver); +SurviveObject *survive_create_ww0(SurviveContext *ctx, const char *driver_name, + void *driver); -int survive_load_htc_config_format(char* ct0conf, int length, SurviveObject * so); +int survive_load_htc_config_format(char *ct0conf, int length, + SurviveObject *so); #endif diff --git a/src/survive_playback.c b/src/survive_playback.c old mode 100755 new mode 100644 index 7c49f5b..4b25b4c --- a/src/survive_playback.c +++ b/src/survive_playback.c @@ -1,8 +1,9 @@ -//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses. +// All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL +// or LGPL licenses. -#include #include #include +#include #include #include @@ -13,221 +14,229 @@ #include "redist/os_generic.h" struct SurvivePlaybackData { - SurviveContext * ctx; - const char* playback_dir; - FILE* playback_file; - int lineno; + SurviveContext *ctx; + const char *playback_dir; + FILE *playback_file; + int lineno; - FLT time_factor; - double next_time_us; + FLT time_factor; + double next_time_us; }; typedef struct SurvivePlaybackData SurvivePlaybackData; - double timestamp_in_us() { - static double start_time_us = 0; - if(start_time_us == 0.) - start_time_us = OGGetAbsoluteTime(); - return OGGetAbsoluteTime() - start_time_us; + static double start_time_us = 0; + if (start_time_us == 0.) + start_time_us = OGGetAbsoluteTime(); + return OGGetAbsoluteTime() - start_time_us; } -static int parse_and_run_imu(const char* line, SurvivePlaybackData* driver) { - char dev[10]; - int timecode = 0; - FLT accelgyro[6]; - int mask; - int id; - - int rr = sscanf(line,"I %s %d %d " FLT_format " " FLT_format " " FLT_format " " FLT_format " " FLT_format " " FLT_format "%d", dev, - &mask, - &timecode, - &accelgyro[0], &accelgyro[1], &accelgyro[2], - &accelgyro[3], &accelgyro[4], &accelgyro[5], &id ); - - if( rr != 10 ) - { - fprintf( stderr, "Warning: On line %d, only %d values read: '%s'\n", driver->lineno, rr, line ); - return -1; - } - - SurviveObject * so = survive_get_so_by_name( driver->ctx, dev); - if(!so) { - fprintf(stderr, "Could not find device named %s from lineno %d\n", dev, driver->lineno); - return -1; - } - - driver->ctx->imuproc( so, mask, accelgyro, timecode, id); - return 0; +static int parse_and_run_imu(const char *line, SurvivePlaybackData *driver) { + char dev[10]; + int timecode = 0; + FLT accelgyro[6]; + int mask; + int id; + + int rr = + sscanf(line, "I %s %d %d " FLT_format " " FLT_format " " FLT_format + " " FLT_format " " FLT_format " " FLT_format "%d", + dev, &mask, &timecode, &accelgyro[0], &accelgyro[1], + &accelgyro[2], &accelgyro[3], &accelgyro[4], &accelgyro[5], &id); + + if (rr != 10) { + fprintf(stderr, "Warning: On line %d, only %d values read: '%s'\n", + driver->lineno, rr, line); + return -1; + } + + SurviveObject *so = survive_get_so_by_name(driver->ctx, dev); + if (!so) { + fprintf(stderr, "Could not find device named %s from lineno %d\n", dev, + driver->lineno); + return -1; + } + + driver->ctx->imuproc(so, mask, accelgyro, timecode, id); + return 0; } +static int parse_and_run_lightcode(const char *line, + SurvivePlaybackData *driver) { + char lhn[10]; + char axn[10]; + char dev[10]; + uint32_t timecode = 0; + int sensor = 0; + int acode = 0; + int timeinsweep = 0; + uint32_t pulselength = 0; + uint32_t lh = 0; + + int rr = + sscanf(line, "%8s %8s %8s %u %d %d %d %u %u\n", lhn, axn, dev, + &timecode, &sensor, &acode, &timeinsweep, &pulselength, &lh); + + if (rr != 9) { + fprintf(stderr, "Warning: On line %d, only %d values read: '%s'\n", + driver->lineno, rr, line); + return -1; + } -static int parse_and_run_lightcode(const char* line, SurvivePlaybackData* driver) { - char lhn[10]; - char axn[10]; - char dev[10]; - uint32_t timecode = 0; - int sensor = 0; - int acode = 0; - int timeinsweep = 0; - uint32_t pulselength = 0; - uint32_t lh = 0; - - int rr = sscanf(line,"%8s %8s %8s %u %d %d %d %u %u\n", - lhn, axn, dev, - &timecode, &sensor, &acode, - &timeinsweep, &pulselength, &lh ); - - if( rr != 9 ) - { - fprintf( stderr, "Warning: On line %d, only %d values read: '%s'\n", driver->lineno, rr, line ); - return -1; - } - - SurviveObject * so = survive_get_so_by_name( driver->ctx, dev); - if(!so) { - fprintf(stderr, "Could not find device named %s from lineno %d\n", dev, driver->lineno); - return -1; + SurviveObject *so = survive_get_so_by_name(driver->ctx, dev); + if (!so) { + fprintf(stderr, "Could not find device named %s from lineno %d\n", dev, + driver->lineno); + return -1; } - - driver->ctx->lightproc( so, sensor, acode, timeinsweep, timecode, pulselength, lh); + + driver->ctx->lightproc(so, sensor, acode, timeinsweep, timecode, + pulselength, lh); return 0; } -static int playback_poll( struct SurviveContext * ctx, void * _driver ) { - SurvivePlaybackData* driver = _driver; - FILE* f = driver->playback_file; - - if(f && !feof(f) && !ferror(f) ) - { - int i; - driver->lineno++; - char * line; - - if(driver->next_time_us == 0) { - char * buffer; - size_t n = 0; - ssize_t r = getdelim( &line, &n, ' ', f ); - if( r <= 0 ) return 0; - - if(sscanf(line, "%lf", &driver->next_time_us) != 1) { - free(line); - return 0; +static int playback_poll(struct SurviveContext *ctx, void *_driver) { + SurvivePlaybackData *driver = _driver; + FILE *f = driver->playback_file; + + if (f && !feof(f) && !ferror(f)) { + int i; + driver->lineno++; + char *line; + + if (driver->next_time_us == 0) { + char *buffer; + size_t n = 0; + ssize_t r = getdelim(&line, &n, ' ', f); + if (r <= 0) + return 0; + + if (sscanf(line, "%lf", &driver->next_time_us) != 1) { + free(line); + return 0; + } + free(line); + line = 0; + } + + if (driver->next_time_us * driver->time_factor > timestamp_in_us()) + return 0; + driver->next_time_us = 0; + + char *buffer; + size_t n = 0; + ssize_t r = getline(&line, &n, f); + if (r <= 0) + return 0; + + if ((line[0] != 'R' && line[0] != 'L' && line[0] != 'I') || + line[1] != ' ') + return 0; + + switch (line[0]) { + case 'L': + case 'R': + parse_and_run_lightcode(line, driver); + break; + case 'I': + parse_and_run_imu(line, driver); + break; + } + + free(line); + } else { + if (f) { + fclose(driver->playback_file); + } + driver->playback_file = 0; + return -1; } - free(line); - line = 0; - } - if(driver->next_time_us * driver->time_factor > timestamp_in_us()) return 0; - driver->next_time_us = 0; - - char * buffer; - size_t n = 0; - ssize_t r = getline( &line, &n, f ); - if( r <= 0 ) return 0; - - if((line[0] != 'R' && line[0] != 'L' && line[0] != 'I') || line[1] != ' ' ) - return 0; - - switch(line[0]) { - case 'L': - case 'R': - parse_and_run_lightcode(line, driver); - break; - case 'I': - parse_and_run_imu(line, driver); - break; - } - - free( line ); - } else { - if(f) { - fclose(driver->playback_file); - } - driver->playback_file = 0; - return -1; - } - - return 0; } -static int playback_close( struct SurviveContext * ctx, void * _driver ) { - SurvivePlaybackData* driver = _driver; - if(driver->playback_file) - fclose(driver->playback_file); - driver->playback_file = 0; - return 0; +static int playback_close(struct SurviveContext *ctx, void *_driver) { + SurvivePlaybackData *driver = _driver; + if (driver->playback_file) + fclose(driver->playback_file); + driver->playback_file = 0; + return 0; } +static int LoadConfig(SurvivePlaybackData *sv, SurviveObject *so) { + SurviveContext *ctx = sv->ctx; + char *ct0conf = 0; -static int LoadConfig( SurvivePlaybackData * sv, SurviveObject * so) -{ - SurviveContext * ctx = sv->ctx; - char * ct0conf = 0; - char fname[100]; - sprintf( fname, "%s/%s_config.json", sv->playback_dir, so->codename ); - FILE * f = fopen( fname, "r" ); + sprintf(fname, "%s/%s_config.json", sv->playback_dir, so->codename); + FILE *f = fopen(fname, "r"); + + if (f == 0 || feof(f) || ferror(f)) + return 1; - if(f == 0 || feof(f) || ferror(f) ) - return 1; - fseek(f, 0, SEEK_END); int len = ftell(f); - fseek(f, 0, SEEK_SET); //same as rewind(f); + fseek(f, 0, SEEK_SET); // same as rewind(f); - ct0conf = malloc(len+1); - int read = fread( ct0conf, len, 1, f); - fclose( f ); + ct0conf = malloc(len + 1); + int read = fread(ct0conf, len, 1, f); + fclose(f); ct0conf[len] = 0; - printf( "Loading config: %d\n", len ); + printf("Loading config: %d\n", len); return survive_load_htc_config_format(ct0conf, len, so); } +int DriverRegPlayback(SurviveContext *ctx) { + const char *playback_dir = + config_read_str(ctx->global_config_values, "PlaybackDir", ""); + if (strlen(playback_dir) == 0) { + return 0; + } -int DriverRegPlayback( SurviveContext * ctx ) -{ - const char* playback_dir = config_read_str(ctx->global_config_values, - "PlaybackDir", ""); - - if(strlen(playback_dir) == 0) { - return 0; - } - - SurvivePlaybackData * sp = calloc( 1, sizeof( SurvivePlaybackData ) ); - sp->ctx = ctx; - sp->playback_dir = playback_dir; - sp->time_factor = config_read_float(ctx->global_config_values, "PlaybackFactor", 1.); - - printf("%s\n", playback_dir); + SurvivePlaybackData *sp = calloc(1, sizeof(SurvivePlaybackData)); + sp->ctx = ctx; + sp->playback_dir = playback_dir; + sp->time_factor = + config_read_float(ctx->global_config_values, "PlaybackFactor", 1.); + + printf("%s\n", playback_dir); + + char playback_file[100]; + sprintf(playback_file, "%s/events", playback_dir); + sp->playback_file = fopen(playback_file, "r"); + if (sp->playback_file == 0) { + fprintf(stderr, "Could not open playback events file %s", + playback_file); + return -1; + } + SurviveObject *hmd = survive_create_hmd(ctx, "Playback", sp); + SurviveObject *wm0 = survive_create_wm0(ctx, "Playback", sp, 0); + SurviveObject *wm1 = survive_create_wm1(ctx, "Playback", sp, 0); + SurviveObject *tr0 = survive_create_tr0(ctx, "Playback", sp); + SurviveObject *ww0 = survive_create_ww0(ctx, "Playback", sp); + + if (!LoadConfig(sp, hmd)) { + survive_add_object(ctx, hmd); + } + if (!LoadConfig(sp, wm0)) { + survive_add_object(ctx, wm0); + } + if (!LoadConfig(sp, wm1)) { + survive_add_object(ctx, wm1); + } + if (!LoadConfig(sp, tr0)) { + survive_add_object(ctx, tr0); + } + if (!LoadConfig(sp, ww0)) { + survive_add_object(ctx, ww0); + } - char playback_file[100]; - sprintf( playback_file, "%s/events", playback_dir ); - sp->playback_file = fopen( playback_file, "r"); - if(sp->playback_file == 0) { - fprintf(stderr, "Could not open playback events file %s", playback_file); - return -1; -} - SurviveObject * hmd = survive_create_hmd(ctx, "Playback", sp); - SurviveObject * wm0 = survive_create_wm0(ctx, "Playback", sp, 0); - SurviveObject * wm1 = survive_create_wm1(ctx, "Playback", sp, 0); - SurviveObject * tr0 = survive_create_tr0(ctx, "Playback", sp); - SurviveObject * ww0 = survive_create_ww0(ctx, "Playback", sp); - - if( !LoadConfig( sp, hmd )) { survive_add_object( ctx, hmd ); } - if( !LoadConfig( sp, wm0 )) { survive_add_object( ctx, wm0 ); } - if( !LoadConfig( sp, wm1 )) { survive_add_object( ctx, wm1 ); } - if( !LoadConfig( sp, tr0 )) { survive_add_object( ctx, tr0 ); } - if( !LoadConfig( sp, ww0 )) { survive_add_object( ctx, ww0 ); } - - - survive_add_driver(ctx, sp, playback_poll, playback_close, 0); - return 0; - fail_gracefully: - return -1; + survive_add_driver(ctx, sp, playback_poll, playback_close, 0); + return 0; +fail_gracefully: + return -1; } -REGISTER_LINKTIME( DriverRegPlayback ); - +REGISTER_LINKTIME(DriverRegPlayback); -- cgit v1.2.3 From b01acf54ed99cb4e491871495f2e18fe78494a36 Mon Sep 17 00:00:00 2001 From: CNLohr Date: Sat, 10 Mar 2018 16:51:56 -0500 Subject: Add notes about how to install on Debian --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index b587d3b..98f75c8 100644 --- a/README.md +++ b/README.md @@ -79,6 +79,8 @@ Will ~~I~~ we succeed? Probably not. ~~Definitely going to try!~~ Though it's * pthread * libX11 (Linux) or Native (win32) or OpenGL (OSX) * zlib (Linux) or puff.c (win32, included in redist) +* Optionally OpenGL. +On Debian, ```sudo apt-get install build-essential zlib1g-dev libx11-dev libusb-1.0-0-dev freeglut3-dev``` should be sufficient. ## Architecture -- cgit v1.2.3 From e96190c63875eb8e622dcf70f3e4571cd8ec6931 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 10 Mar 2018 17:21:15 -0500 Subject: Forgot to add hid-linux.c --- redist/hid-linux.c | 798 +++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 798 insertions(+) create mode 100644 redist/hid-linux.c diff --git a/redist/hid-linux.c b/redist/hid-linux.c new file mode 100644 index 0000000..5cc3bf2 --- /dev/null +++ b/redist/hid-linux.c @@ -0,0 +1,798 @@ +/******************************************************* + HIDAPI - Multi-Platform library for + communication with HID devices. + + Alan Ott + Signal 11 Software + + 8/22/2009 + Linux Version - 6/2/2009 + + Copyright 2009, All Rights Reserved. + + At the discretion of the user of this library, + this software may be licensed under the terms of the + GNU General Public License v3, a BSD-Style license, or the + original HIDAPI license as outlined in the LICENSE.txt, + LICENSE-gpl3.txt, LICENSE-bsd.txt, and LICENSE-orig.txt + files located at the root of the source distribution. + These files may also be found in the public source + code repository located at: + http://github.com/signal11/hidapi . +********************************************************/ + +/* C */ +#include +#include +#include +#include +#include + +/* Unix */ +#include +#include +#include +#include +#include +#include +#include + +/* Linux */ +#include +#include +#include +#include + +#include "hidapi.h" + +/* Definitions from linux/hidraw.h. Since these are new, some distros + may not have header files which contain them. */ +#ifndef HIDIOCSFEATURE +#define HIDIOCSFEATURE(len) _IOC(_IOC_WRITE|_IOC_READ, 'H', 0x06, len) +#endif +#ifndef HIDIOCGFEATURE +#define HIDIOCGFEATURE(len) _IOC(_IOC_WRITE|_IOC_READ, 'H', 0x07, len) +#endif + + +/* USB HID device property names */ +const char *device_string_names[] = { + "manufacturer", + "product", + "serial", +}; + +/* Symbolic names for the properties above */ +enum device_string_id { + DEVICE_STRING_MANUFACTURER, + DEVICE_STRING_PRODUCT, + DEVICE_STRING_SERIAL, + + DEVICE_STRING_COUNT, +}; + +struct hid_device_ { + int device_handle; + int blocking; + int uses_numbered_reports; +}; + + +static __u32 kernel_version = 0; + +static __u32 detect_kernel_version(void) +{ + struct utsname name; + int major, minor, release; + int ret; + + uname(&name); + ret = sscanf(name.release, "%d.%d.%d", &major, &minor, &release); + if (ret == 3) { + return KERNEL_VERSION(major, minor, release); + } + + ret = sscanf(name.release, "%d.%d", &major, &minor); + if (ret == 2) { + return KERNEL_VERSION(major, minor, 0); + } + + printf("Couldn't determine kernel version from version string \"%s\"\n", name.release); + return 0; +} + +static hid_device *new_hid_device(void) +{ + hid_device *dev = calloc(1, sizeof(hid_device)); + dev->device_handle = -1; + dev->blocking = 1; + dev->uses_numbered_reports = 0; + + return dev; +} + + +/* The caller must free the returned string with free(). */ +static wchar_t *utf8_to_wchar_t(const char *utf8) +{ + wchar_t *ret = NULL; + + if (utf8) { + size_t wlen = mbstowcs(NULL, utf8, 0); + if ((size_t) -1 == wlen) { + return wcsdup(L""); + } + ret = calloc(wlen+1, sizeof(wchar_t)); + mbstowcs(ret, utf8, wlen+1); + ret[wlen] = 0x0000; + } + + return ret; +} + +/* Get an attribute value from a udev_device and return it as a whar_t + string. The returned string must be freed with free() when done.*/ +static wchar_t *copy_udev_string(struct udev_device *dev, const char *udev_name) +{ + return utf8_to_wchar_t(udev_device_get_sysattr_value(dev, udev_name)); +} + +/* uses_numbered_reports() returns 1 if report_descriptor describes a device + which contains numbered reports. */ +static int uses_numbered_reports(__u8 *report_descriptor, __u32 size) { + unsigned int i = 0; + int size_code; + int data_len, key_size; + + while (i < size) { + int key = report_descriptor[i]; + + /* Check for the Report ID key */ + if (key == 0x85/*Report ID*/) { + /* This device has a Report ID, which means it uses + numbered reports. */ + return 1; + } + + //printf("key: %02hhx\n", key); + + if ((key & 0xf0) == 0xf0) { + /* This is a Long Item. The next byte contains the + length of the data section (value) for this key. + See the HID specification, version 1.11, section + 6.2.2.3, titled "Long Items." */ + if (i+1 < size) + data_len = report_descriptor[i+1]; + else + data_len = 0; /* malformed report */ + key_size = 3; + } + else { + /* This is a Short Item. The bottom two bits of the + key contain the size code for the data section + (value) for this key. Refer to the HID + specification, version 1.11, section 6.2.2.2, + titled "Short Items." */ + size_code = key & 0x3; + switch (size_code) { + case 0: + case 1: + case 2: + data_len = size_code; + break; + case 3: + data_len = 4; + break; + default: + /* Can't ever happen since size_code is & 0x3 */ + data_len = 0; + break; + }; + key_size = 1; + } + + /* Skip over this key and it's associated data */ + i += data_len + key_size; + } + + /* Didn't find a Report ID key. Device doesn't use numbered reports. */ + return 0; +} + +/* + * The caller is responsible for free()ing the (newly-allocated) character + * strings pointed to by serial_number_utf8 and product_name_utf8 after use. + */ +static int +parse_uevent_info(const char *uevent, int *bus_type, + unsigned short *vendor_id, unsigned short *product_id, + char **serial_number_utf8, char **product_name_utf8) +{ + char *tmp = strdup(uevent); + char *saveptr = NULL; + char *line; + char *key; + char *value; + + int found_id = 0; + int found_serial = 0; + int found_name = 0; + + line = strtok_r(tmp, "\n", &saveptr); + while (line != NULL) { + /* line: "KEY=value" */ + key = line; + value = strchr(line, '='); + if (!value) { + goto next_line; + } + *value = '\0'; + value++; + + if (strcmp(key, "HID_ID") == 0) { + /** + * type vendor product + * HID_ID=0003:000005AC:00008242 + **/ + int ret = sscanf(value, "%x:%hx:%hx", bus_type, vendor_id, product_id); + if (ret == 3) { + found_id = 1; + } + } else if (strcmp(key, "HID_NAME") == 0) { + /* The caller has to free the product name */ + *product_name_utf8 = strdup(value); + found_name = 1; + } else if (strcmp(key, "HID_UNIQ") == 0) { + /* The caller has to free the serial number */ + *serial_number_utf8 = strdup(value); + found_serial = 1; + } + +next_line: + line = strtok_r(NULL, "\n", &saveptr); + } + + free(tmp); + return (found_id && found_name && found_serial); +} + + +static int get_device_string(hid_device *dev, enum device_string_id key, wchar_t *string, size_t maxlen) +{ + struct udev *udev; + struct udev_device *udev_dev, *parent, *hid_dev; + struct stat s; + int ret = -1; + char *serial_number_utf8 = NULL; + char *product_name_utf8 = NULL; + + /* Create the udev object */ + udev = udev_new(); + if (!udev) { + printf("Can't create udev\n"); + return -1; + } + + /* Get the dev_t (major/minor numbers) from the file handle. */ + ret = fstat(dev->device_handle, &s); + if (-1 == ret) + return ret; + /* Open a udev device from the dev_t. 'c' means character device. */ + udev_dev = udev_device_new_from_devnum(udev, 'c', s.st_rdev); + if (udev_dev) { + hid_dev = udev_device_get_parent_with_subsystem_devtype( + udev_dev, + "hid", + NULL); + if (hid_dev) { + unsigned short dev_vid; + unsigned short dev_pid; + int bus_type; + size_t retm; + + ret = parse_uevent_info( + udev_device_get_sysattr_value(hid_dev, "uevent"), + &bus_type, + &dev_vid, + &dev_pid, + &serial_number_utf8, + &product_name_utf8); + + if (bus_type == BUS_BLUETOOTH) { + switch (key) { + case DEVICE_STRING_MANUFACTURER: + wcsncpy(string, L"", maxlen); + ret = 0; + break; + case DEVICE_STRING_PRODUCT: + retm = mbstowcs(string, product_name_utf8, maxlen); + ret = (retm == (size_t)-1)? -1: 0; + break; + case DEVICE_STRING_SERIAL: + retm = mbstowcs(string, serial_number_utf8, maxlen); + ret = (retm == (size_t)-1)? -1: 0; + break; + case DEVICE_STRING_COUNT: + default: + ret = -1; + break; + } + } + else { + /* This is a USB device. Find its parent USB Device node. */ + parent = udev_device_get_parent_with_subsystem_devtype( + udev_dev, + "usb", + "usb_device"); + if (parent) { + const char *str; + const char *key_str = NULL; + + if (key >= 0 && key < DEVICE_STRING_COUNT) { + key_str = device_string_names[key]; + } else { + ret = -1; + goto end; + } + + str = udev_device_get_sysattr_value(parent, key_str); + if (str) { + /* Convert the string from UTF-8 to wchar_t */ + retm = mbstowcs(string, str, maxlen); + ret = (retm == (size_t)-1)? -1: 0; + goto end; + } + } + } + } + } + +end: + free(serial_number_utf8); + free(product_name_utf8); + + udev_device_unref(udev_dev); + /* parent and hid_dev don't need to be (and can't be) unref'd. + I'm not sure why, but they'll throw double-free() errors. */ + udev_unref(udev); + + return ret; +} + +int HID_API_EXPORT hid_init(void) +{ + const char *locale; + + /* Set the locale if it's not set. */ + locale = setlocale(LC_CTYPE, NULL); + if (!locale) + setlocale(LC_CTYPE, ""); + + kernel_version = detect_kernel_version(); + + return 0; +} + +int HID_API_EXPORT hid_exit(void) +{ + /* Nothing to do for this in the Linux/hidraw implementation. */ + return 0; +} + + +struct hid_device_info HID_API_EXPORT *hid_enumerate(unsigned short vendor_id, unsigned short product_id) +{ + struct udev *udev; + struct udev_enumerate *enumerate; + struct udev_list_entry *devices, *dev_list_entry; + + struct hid_device_info *root = NULL; /* return object */ + struct hid_device_info *cur_dev = NULL; + struct hid_device_info *prev_dev = NULL; /* previous device */ + + hid_init(); + + /* Create the udev object */ + udev = udev_new(); + if (!udev) { + printf("Can't create udev\n"); + return NULL; + } + + /* Create a list of the devices in the 'hidraw' subsystem. */ + enumerate = udev_enumerate_new(udev); + udev_enumerate_add_match_subsystem(enumerate, "hidraw"); + udev_enumerate_scan_devices(enumerate); + devices = udev_enumerate_get_list_entry(enumerate); + /* For each item, see if it matches the vid/pid, and if so + create a udev_device record for it */ + udev_list_entry_foreach(dev_list_entry, devices) { + const char *sysfs_path; + const char *dev_path; + const char *str; + struct udev_device *raw_dev; /* The device's hidraw udev node. */ + struct udev_device *hid_dev; /* The device's HID udev node. */ + struct udev_device *usb_dev; /* The device's USB udev node. */ + struct udev_device *intf_dev; /* The device's interface (in the USB sense). */ + unsigned short dev_vid; + unsigned short dev_pid; + char *serial_number_utf8 = NULL; + char *product_name_utf8 = NULL; + int bus_type; + int result; + + /* Get the filename of the /sys entry for the device + and create a udev_device object (dev) representing it */ + sysfs_path = udev_list_entry_get_name(dev_list_entry); + raw_dev = udev_device_new_from_syspath(udev, sysfs_path); + dev_path = udev_device_get_devnode(raw_dev); + + hid_dev = udev_device_get_parent_with_subsystem_devtype( + raw_dev, + "hid", + NULL); + + if (!hid_dev) { + /* Unable to find parent hid device. */ + goto next; + } + + result = parse_uevent_info( + udev_device_get_sysattr_value(hid_dev, "uevent"), + &bus_type, + &dev_vid, + &dev_pid, + &serial_number_utf8, + &product_name_utf8); + + if (!result) { + /* parse_uevent_info() failed for at least one field. */ + goto next; + } + + if (bus_type != BUS_USB && bus_type != BUS_BLUETOOTH) { + /* We only know how to handle USB and BT devices. */ + goto next; + } + + /* Check the VID/PID against the arguments */ + if ((vendor_id == 0x0 || vendor_id == dev_vid) && + (product_id == 0x0 || product_id == dev_pid)) { + struct hid_device_info *tmp; + + /* VID/PID match. Create the record. */ + tmp = malloc(sizeof(struct hid_device_info)); + if (cur_dev) { + cur_dev->next = tmp; + } + else { + root = tmp; + } + prev_dev = cur_dev; + cur_dev = tmp; + + /* Fill out the record */ + cur_dev->next = NULL; + cur_dev->path = dev_path? strdup(dev_path): NULL; + + /* VID/PID */ + cur_dev->vendor_id = dev_vid; + cur_dev->product_id = dev_pid; + + /* Serial Number */ + cur_dev->serial_number = utf8_to_wchar_t(serial_number_utf8); + + /* Release Number */ + cur_dev->release_number = 0x0; + + /* Interface Number */ + cur_dev->interface_number = -1; + + switch (bus_type) { + case BUS_USB: + /* The device pointed to by raw_dev contains information about + the hidraw device. In order to get information about the + USB device, get the parent device with the + subsystem/devtype pair of "usb"/"usb_device". This will + be several levels up the tree, but the function will find + it. */ + usb_dev = udev_device_get_parent_with_subsystem_devtype( + raw_dev, + "usb", + "usb_device"); + + if (!usb_dev) { + /* Free this device */ + free(cur_dev->serial_number); + free(cur_dev->path); + free(cur_dev); + + /* Take it off the device list. */ + if (prev_dev) { + prev_dev->next = NULL; + cur_dev = prev_dev; + } + else { + cur_dev = root = NULL; + } + + goto next; + } + + /* Manufacturer and Product strings */ + cur_dev->manufacturer_string = copy_udev_string(usb_dev, device_string_names[DEVICE_STRING_MANUFACTURER]); + cur_dev->product_string = copy_udev_string(usb_dev, device_string_names[DEVICE_STRING_PRODUCT]); + + /* Release Number */ + str = udev_device_get_sysattr_value(usb_dev, "bcdDevice"); + cur_dev->release_number = (str)? strtol(str, NULL, 16): 0x0; + + /* Get a handle to the interface's udev node. */ + intf_dev = udev_device_get_parent_with_subsystem_devtype( + raw_dev, + "usb", + "usb_interface"); + if (intf_dev) { + str = udev_device_get_sysattr_value(intf_dev, "bInterfaceNumber"); + cur_dev->interface_number = (str)? strtol(str, NULL, 16): -1; + } + + break; + + case BUS_BLUETOOTH: + /* Manufacturer and Product strings */ + cur_dev->manufacturer_string = wcsdup(L""); + cur_dev->product_string = utf8_to_wchar_t(product_name_utf8); + + break; + + default: + /* Unknown device type - this should never happen, as we + * check for USB and Bluetooth devices above */ + break; + } + } + + next: + free(serial_number_utf8); + free(product_name_utf8); + udev_device_unref(raw_dev); + /* hid_dev, usb_dev and intf_dev don't need to be (and can't be) + unref()d. It will cause a double-free() error. I'm not + sure why. */ + } + /* Free the enumerator and udev objects. */ + udev_enumerate_unref(enumerate); + udev_unref(udev); + + return root; +} + +void HID_API_EXPORT hid_free_enumeration(struct hid_device_info *devs) +{ + struct hid_device_info *d = devs; + while (d) { + struct hid_device_info *next = d->next; + free(d->path); + free(d->serial_number); + free(d->manufacturer_string); + free(d->product_string); + free(d); + d = next; + } +} + +hid_device * hid_open(unsigned short vendor_id, unsigned short product_id, const wchar_t *serial_number) +{ + struct hid_device_info *devs, *cur_dev; + const char *path_to_open = NULL; + hid_device *handle = NULL; + + devs = hid_enumerate(vendor_id, product_id); + cur_dev = devs; + while (cur_dev) { + if (cur_dev->vendor_id == vendor_id && + cur_dev->product_id == product_id) { + if (serial_number) { + if (wcscmp(serial_number, cur_dev->serial_number) == 0) { + path_to_open = cur_dev->path; + break; + } + } + else { + path_to_open = cur_dev->path; + break; + } + } + cur_dev = cur_dev->next; + } + + if (path_to_open) { + /* Open the device */ + handle = hid_open_path(path_to_open); + } + + hid_free_enumeration(devs); + + return handle; +} + +hid_device * HID_API_EXPORT hid_open_path(const char *path) +{ + hid_device *dev = NULL; + + hid_init(); + + dev = new_hid_device(); + + /* OPEN HERE */ + dev->device_handle = open(path, O_RDWR); + + /* If we have a good handle, return it. */ + if (dev->device_handle > 0) { + + /* Get the report descriptor */ + int res, desc_size = 0; + struct hidraw_report_descriptor rpt_desc; + + memset(&rpt_desc, 0x0, sizeof(rpt_desc)); + + /* Get Report Descriptor Size */ + res = ioctl(dev->device_handle, HIDIOCGRDESCSIZE, &desc_size); + if (res < 0) + perror("HIDIOCGRDESCSIZE"); + + + /* Get Report Descriptor */ + rpt_desc.size = desc_size; + res = ioctl(dev->device_handle, HIDIOCGRDESC, &rpt_desc); + if (res < 0) { + perror("HIDIOCGRDESC"); + } else { + /* Determine if this device uses numbered reports. */ + dev->uses_numbered_reports = + uses_numbered_reports(rpt_desc.value, + rpt_desc.size); + } + + return dev; + } + else { + /* Unable to open any devices. */ + free(dev); + return NULL; + } +} + + +int HID_API_EXPORT hid_write(hid_device *dev, const unsigned char *data, size_t length) +{ + int bytes_written; + + bytes_written = write(dev->device_handle, data, length); + + return bytes_written; +} + + +int HID_API_EXPORT hid_read_timeout(hid_device *dev, unsigned char *data, size_t length, int milliseconds) +{ + int bytes_read; + + if (milliseconds >= 0) { + /* Milliseconds is either 0 (non-blocking) or > 0 (contains + a valid timeout). In both cases we want to call poll() + and wait for data to arrive. Don't rely on non-blocking + operation (O_NONBLOCK) since some kernels don't seem to + properly report device disconnection through read() when + in non-blocking mode. */ + int ret; + struct pollfd fds; + + fds.fd = dev->device_handle; + fds.events = POLLIN; + fds.revents = 0; + ret = poll(&fds, 1, milliseconds); + if (ret == -1 || ret == 0) { + /* Error or timeout */ + return ret; + } + else { + /* Check for errors on the file descriptor. This will + indicate a device disconnection. */ + if (fds.revents & (POLLERR | POLLHUP | POLLNVAL)) + return -1; + } + } + + bytes_read = read(dev->device_handle, data, length); + if (bytes_read < 0 && (errno == EAGAIN || errno == EINPROGRESS)) + bytes_read = 0; + + if (bytes_read >= 0 && + kernel_version != 0 && + kernel_version < KERNEL_VERSION(2,6,34) && + dev->uses_numbered_reports) { + /* Work around a kernel bug. Chop off the first byte. */ + memmove(data, data+1, bytes_read); + bytes_read--; + } + + return bytes_read; +} + +int HID_API_EXPORT hid_read(hid_device *dev, unsigned char *data, size_t length) +{ + return hid_read_timeout(dev, data, length, (dev->blocking)? -1: 0); +} + +int HID_API_EXPORT hid_set_nonblocking(hid_device *dev, int nonblock) +{ + /* Do all non-blocking in userspace using poll(), since it looks + like there's a bug in the kernel in some versions where + read() will not return -1 on disconnection of the USB device */ + + dev->blocking = !nonblock; + return 0; /* Success */ +} + + +int HID_API_EXPORT hid_send_feature_report(hid_device *dev, const unsigned char *data, size_t length) +{ + int res; + + res = ioctl(dev->device_handle, HIDIOCSFEATURE(length), data); + if (res < 0) + perror("ioctl (SFEATURE)"); + + return res; +} + +int HID_API_EXPORT hid_get_feature_report(hid_device *dev, unsigned char *data, size_t length) +{ + int res; + + res = ioctl(dev->device_handle, HIDIOCGFEATURE(length), data); + if (res < 0) + perror("ioctl (GFEATURE)"); + + + return res; +} + + +void HID_API_EXPORT hid_close(hid_device *dev) +{ + if (!dev) + return; + close(dev->device_handle); + free(dev); +} + + +int HID_API_EXPORT_CALL hid_get_manufacturer_string(hid_device *dev, wchar_t *string, size_t maxlen) +{ + return get_device_string(dev, DEVICE_STRING_MANUFACTURER, string, maxlen); +} + +int HID_API_EXPORT_CALL hid_get_product_string(hid_device *dev, wchar_t *string, size_t maxlen) +{ + return get_device_string(dev, DEVICE_STRING_PRODUCT, string, maxlen); +} + +int HID_API_EXPORT_CALL hid_get_serial_number_string(hid_device *dev, wchar_t *string, size_t maxlen) +{ + return get_device_string(dev, DEVICE_STRING_SERIAL, string, maxlen); +} + +int HID_API_EXPORT_CALL hid_get_indexed_string(hid_device *dev, int string_index, wchar_t *string, size_t maxlen) +{ + return -1; +} + + +HID_API_EXPORT const wchar_t * HID_API_CALL hid_error(hid_device *dev) +{ + return NULL; +} + -- cgit v1.2.3 From 33eb803fe6b5eb2fc2e02e673e6e641e72592c94 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 10 Mar 2018 17:51:27 -0500 Subject: Fix non-HIDAPI usage in some situations on older udev platforms. --- useful_files/81-vive.rules | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/useful_files/81-vive.rules b/useful_files/81-vive.rules index ab38087..652b599 100644 --- a/useful_files/81-vive.rules +++ b/useful_files/81-vive.rules @@ -12,3 +12,13 @@ SUBSYSTEM=="usb", ATTRS{idVendor}=="114d", ATTRS{idProduct}=="8328", TAG+="uacce # HTC Mass Storage Node SUBSYSTEM=="usb", ATTRS{idVendor}=="114d", ATTRS{idProduct}=="8200", TAG+="uaccess" SUBSYSTEM=="usb", ATTRS{idVendor}=="114d", ATTRS{idProduct}=="8a12", TAG+="uaccess" + + +#libsurvive +SUBSYSTEM=="usb", ATTR{idVendor}=="0bb4", ATTR{idProduct}=="2c87", MODE="0666" # HTC HMD +SUBSYSTEM=="usb", ATTR{idVendor}=="28de", ATTR{idProduct}=="2000", MODE="0666" # Light input +SUBSYSTEM=="usb", ATTR{idVendor}=="28de", ATTR{idProduct}=="2101", MODE="0666" # Watchman + +#KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="0bb4", ATTR{idProduct}=="2c87", MODE="0666" # HTC +#KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="28de", ATTR{idProduct}=="2000", MODE="0666" # Valve +#KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="28de", ATTR{idProduct}=="2101", MODE="0666" # Valve -- cgit v1.2.3 From b0856a49eb1bb693fa7bb2e1a090a1420b85a95e Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 10 Mar 2018 18:10:29 -0500 Subject: Whoops. This should _never_ print to stdout. --- src/survive_vive.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/survive_vive.c b/src/survive_vive.c index 288f8cb..b3d990a 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -446,7 +446,6 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject * if( d == 0 ) { - printf( "!!%p %d %04x %04x %d\n", devnames[i], i, vid, pid, which ); SV_INFO( "Did not find device %s (%04x:%04x.%d)", devnames[i], vid, pid, which ); sv->udev[i] = 0; continue; -- cgit v1.2.3