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. --- src/survive.c | 2 ++ src/survive_cal.c | 4 ++-- 2 files changed, 4 insertions(+), 2 deletions(-) (limited to 'src') 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(+) (limited to 'src') 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(-) (limited to 'src') 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(-) (limited to 'src') 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 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. --- src/poser_turveytori.c | 150 ++++++++++++++++++++++++++++++++++++++++--------- 1 file changed, 122 insertions(+), 28 deletions(-) (limited to 'src') 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(-) (limited to 'src') 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(-) (limited to 'src') 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(-) (limited to 'src') 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. --- src/survive.c | 55 +++++++++++++ src/survive_vive.c | 233 +++++++++++++++++++++++++++++++++++++++++++++++------ 2 files changed, 262 insertions(+), 26 deletions(-) (limited to 'src') 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(-) (limited to 'src') 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(-) (limited to 'src') 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. --- src/survive.c | 36 +++++++++++++++++++++++++++++------- src/survive_process.c | 11 +++++++++++ src/survive_vive.c | 28 ++++++++++++++-------------- 3 files changed, 54 insertions(+), 21 deletions(-) (limited to 'src') 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 --- src/survive.c | 13 +++++++- src/survive_process.c | 26 ++++++++++----- src/survive_vive.c | 87 +++++++++++++++++++++++++++++++++++++++++++++------ 3 files changed, 108 insertions(+), 18 deletions(-) (limited to 'src') 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. --- src/poser_turveytori.c | 8 ++++++-- src/survive.c | 9 ++++++++- src/survive_data.c | 4 ++-- src/survive_process.c | 39 ++++++++++++++++++++++++++++++--------- 4 files changed, 46 insertions(+), 14 deletions(-) (limited to 'src') 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 +- 2 files changed, 13 insertions(+), 1 deletion(-) (limited to 'src') 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]); } -- 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(-) (limited to 'src') 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 +++++++++++++++++++++++++-------------------------- 1 file changed, 34 insertions(+), 34 deletions(-) (limited to 'src') 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]); } -- 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(-) (limited to 'src') 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 --- src/poser_octavioradii.c | 6 +++--- src/survive.c | 2 +- src/survive_vive.c | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) (limited to 'src') 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 --- src/poser_octavioradii.c | 2 +- src/poser_turveytori.c | 5 ++--- src/survive.c | 10 ++++++---- src/survive_config.c | 2 +- 4 files changed, 10 insertions(+), 9 deletions(-) (limited to 'src') 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 --- src/ootx_decoder.c | 2 +- src/poser_turveytori.c | 5 +++-- 2 files changed, 4 insertions(+), 3 deletions(-) (limited to 'src') 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 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(-) (limited to 'src') 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(-) (limited to 'src') 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(-) (limited to 'src') 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 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(-) (limited to 'src') 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 --- src/survive.c | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) (limited to 'src') 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(-) (limited to 'src') 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 --- src/survive_default_devices.c | 46 ++++++ src/survive_default_devices.h | 12 ++ src/survive_playback.c | 374 ++++++++++++++++++++++++++++++++++++++++++ src/survive_vive.c | 101 +++--------- 4 files changed, 459 insertions(+), 74 deletions(-) create mode 100644 src/survive_default_devices.c create mode 100644 src/survive_default_devices.h create mode 100755 src/survive_playback.c (limited to 'src') 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(-) (limited to 'src') 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(-) (limited to 'src') 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(-) (limited to 'src') 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 --- src/survive_playback.c | 20 +++++++++----------- 1 file changed, 9 insertions(+), 11 deletions(-) (limited to 'src') 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) --- src/survive_default_devices.c | 309 +++++++++++++++++----------------- src/survive_default_devices.h | 18 +- src/survive_playback.c | 375 +++++++++++++++++++++--------------------- 3 files changed, 358 insertions(+), 344 deletions(-) mode change 100755 => 100644 src/survive_playback.c (limited to 'src') 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 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(-) (limited to 'src') 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