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_config.c | 52 +++++++++++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 49 insertions(+), 3 deletions(-) (limited to 'src/survive_config.c') 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; } -- cgit v1.3.1 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/survive_config.c') 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.3.1 From 7817da63526f35d10d6d4f6b3d9c02280719e023 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Mon, 15 Jan 2018 21:50:10 -0700 Subject: Fix compiler warnings --- include/libsurvive/survive.h | 5 ++--- redist/json_helpers.c | 2 +- src/poser_octavioradii.c | 2 +- src/poser_turveytori.c | 5 ++--- src/survive.c | 10 ++++++---- src/survive_config.c | 2 +- 6 files changed, 13 insertions(+), 13 deletions(-) (limited to 'src/survive_config.c') diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 4821e63..3d0d472 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -4,7 +4,6 @@ #include #include "survive_types.h" #include "poser.h" -#include "os_generic.h" #ifdef __cplusplus extern "C" { @@ -117,7 +116,7 @@ typedef struct { uint8_t nextReadIndex; //init to 0 uint8_t nextWriteIndex; // init to 0 - og_sema_t buttonservicesem; + void* buttonservicesem; ButtonQueueEntry entry[BUTTON_QUEUE_MAX_LEN]; } ButtonQueue; @@ -150,7 +149,7 @@ struct SurviveContext uint8_t isClosing; // flag to indicate if threads should terminate themselves - og_thread_t buttonservicethread; + void* buttonservicethread; ButtonQueue buttonQueue; }; diff --git a/redist/json_helpers.c b/redist/json_helpers.c index af7ddda..8da841b 100644 --- a/redist/json_helpers.c +++ b/redist/json_helpers.c @@ -121,7 +121,7 @@ char* load_file_to_mem(const char* path) { fseek( f, 0, SEEK_SET ); char * JSON_STRING = malloc( len + 1); memset(JSON_STRING,0,len+1); - int i = fread( JSON_STRING, len, 1, f ); i = i; //Ignore return value. + size_t i = fread( JSON_STRING, len, 1, f ); i = i; //Ignore return value. fclose( f ); return JSON_STRING; } diff --git a/src/poser_octavioradii.c b/src/poser_octavioradii.c index be1e9f7..5805e1a 100644 --- a/src/poser_octavioradii.c +++ b/src/poser_octavioradii.c @@ -591,7 +591,7 @@ int PoserOctavioRadii( SurviveObject * so, PoserData * pd ) } else { - dd->hitCount[i][l->lh][axis] *= 0.5; + dd->hitCount[i][l->lh][axis] = (int)((double)dd->hitCount[i][l->lh][axis] * 0.5); } } //if (0 == l->lh) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 59ff25e..de1516a 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1,5 +1,5 @@ #include -#include +#include "survive_config.h" #include #include #include @@ -785,7 +785,6 @@ FLT RotationEstimateFitnessQuaternion(Point lhPoint, FLT *quaternion, TrackedObj FLT throwaway = RotationEstimateFitnessAxisAngle(lhPoint, axisAngle, obj); - int a = throwaway; return throwaway; } @@ -1397,7 +1396,7 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob SolveForRotationQuat(rotQuat, obj, refinedEstimateGd); SolveForRotation(rot, obj, refinedEstimateGd); FLT objPos[3]; - FLT objPos2[3]; + //FLT objPos2[3]; //{ // toriData->lastLhRotQuat[lh][0] = rotQuat[0]; diff --git a/src/survive.c b/src/survive.c index a5ca68f..0386275 100755 --- a/src/survive.c +++ b/src/survive.c @@ -8,6 +8,7 @@ #include #include "survive_config.h" +#include "os_generic.h" #ifdef __APPLE__ #define z_const const @@ -40,7 +41,7 @@ static void survivenote( struct SurviveContext * ctx, const char * fault ) fprintf( stderr, "Info: %s\n", fault ); } -static void button_servicer(void * context) +static void *button_servicer(void * context) { SurviveContext *ctx = (SurviveContext*)context; @@ -51,7 +52,7 @@ static void button_servicer(void * context) if (ctx->isClosing) { // we're shutting down. Close. - return; + return NULL; } ButtonQueueEntry *entry = &(ctx->buttonQueue.entry[ctx->buttonQueue.nextReadIndex]); @@ -61,7 +62,7 @@ static void button_servicer(void * context) // the buttonQueue // if it does happen, it will kill all future button input printf("ERROR: Unpopulated ButtonQueueEntry! NextReadIndex=%d\n", ctx->buttonQueue.nextReadIndex); - return; + return NULL; } //printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n", @@ -90,6 +91,7 @@ static void button_servicer(void * context) ctx->buttonQueue.nextReadIndex = 0; } }; + return NULL; } SurviveContext * survive_init( int headless ) @@ -182,7 +184,7 @@ SurviveContext * survive_init( int headless ) ctx->buttonQueue.buttonservicesem = OGCreateSema(); // start the thread to process button data - ctx->buttonservicethread = OGCreateThread(&button_servicer, ctx); + ctx->buttonservicethread = OGCreateThread(button_servicer, ctx); survive_install_button_fn(ctx, NULL); survive_install_raw_pose_fn(ctx, NULL); diff --git a/src/survive_config.c b/src/survive_config.c index 0961651..46c6658 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -187,7 +187,7 @@ uint16_t config_read_float_array(config_group *cg, const char *tag, FLT* values, config_entry *cv = find_config_entry(cg, tag); if (cv != NULL) { - for (int i=0; i < CFG_MIN(count, cv->elements); i++) + for (unsigned int i=0; i < CFG_MIN(count, cv->elements); i++) { values[i] = ((double*)cv->data)[i]; } -- cgit v1.3.1