From 95eab1b8938e0033a0aa0d10d6ec53d7d2782907 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 11 Mar 2018 15:31:28 -0600 Subject: Added callback for lighthouse found; example in test.c --- include/libsurvive/survive.h | 3 +++ include/libsurvive/survive_types.h | 1 + src/poser_charlesslow.c | 20 ++++++++++++-------- src/poser_turveytori.c | 20 ++++++++++---------- src/survive.c | 10 +++++++++- src/survive_cal.c | 4 ---- src/survive_process.c | 13 +++++++++++++ test.c | 8 ++++++++ 8 files changed, 56 insertions(+), 23 deletions(-) diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 58462b3..03a71bc 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -129,6 +129,7 @@ struct SurviveContext angle_process_func angleproc; button_process_func buttonproc; raw_pose_func rawposeproc; + lighthouse_pose_func lighthouseposeproc; struct config_group* global_config_values; struct config_group* lh_config; //lighthouse configs @@ -175,6 +176,7 @@ void survive_install_imu_fn( SurviveContext * ctx, imu_process_func fbp ); void survive_install_angle_fn( SurviveContext * ctx, angle_process_func fbp ); void survive_install_button_fn(SurviveContext * ctx, button_process_func fbp); void survive_install_raw_pose_fn(SurviveContext * ctx, raw_pose_func fbp); +void survive_install_lighthouse_pose_fn(SurviveContext *ctx, lighthouse_pose_func fbp); void survive_close( SurviveContext * ctx ); int survive_poll( SurviveContext * ctx ); @@ -202,6 +204,7 @@ void survive_default_imu_process( SurviveObject * so, int mode, FLT * accelgyro, void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh ); void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val); void survive_default_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose); +void survive_default_lighthouse_pose_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *pose); ////////////////////// Survive Drivers //////////////////////////// diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h index 4db76b4..ce0baac 100644 --- a/include/libsurvive/survive_types.h +++ b/include/libsurvive/survive_types.h @@ -50,6 +50,7 @@ typedef void (*imu_process_func)( SurviveObject * so, int mask, FLT * accelgyro, typedef void (*angle_process_func)( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh); typedef void(*button_process_func)(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val); typedef void (*raw_pose_func)(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose); +typedef void (*lighthouse_pose_func)(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *pose); typedef int(*haptic_func)(SurviveObject * so, uint8_t reserved, uint16_t pulseHigh , uint16_t pulseLow, uint16_t repeatCount); diff --git a/src/poser_charlesslow.c b/src/poser_charlesslow.c index 96442b3..30fc677 100644 --- a/src/poser_charlesslow.c +++ b/src/poser_charlesslow.c @@ -165,9 +165,18 @@ int PoserCharlesSlow( SurviveObject * so, PoserData * pd ) RunOpti(so, fs, lh, 1, LighthousePos, LighthouseQuat); - ctx->bsd[lh].PositionSet = 1; - copy3d( ctx->bsd[lh].Pose.Pos, LighthousePos ); - quatcopy( ctx->bsd[lh].Pose.Rot, LighthouseQuat ); + SurvivePose lighthousePose; + copy3d(lighthousePose.Pos, LighthousePos); + quatcopy(lighthousePose.Rot, LighthouseQuat); + + const FLT rt[4] = {0, 0, 1, 0}; + FLT tmp[4]; + quatrotateabout(tmp, lighthousePose.Rot, rt); + memcpy(lighthousePose.Rot, tmp, sizeof(FLT) * 4); + + if (ctx->lighthouseposeproc) { + ctx->lighthouseposeproc(ctx, lh, &lighthousePose); + } #define ALT_COORDS #ifdef ALT_COORDS @@ -189,11 +198,6 @@ int PoserCharlesSlow( SurviveObject * so, PoserData * pd ) so->FromLHPose[lh].Rot[2] = LighthouseQuat[2]; so->FromLHPose[lh].Rot[3] = LighthouseQuat[3]; #endif - - const FLT rt[4] = {0, 0, 1, 0}; - FLT tmp[4]; - quatrotateabout(tmp, so->ctx->bsd[lh].Pose.Rot, rt); - memcpy(so->ctx->bsd[lh].Pose.Rot, tmp, sizeof(FLT) * 4); } return 0; diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index dae51e6..ddc4ad2 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1436,17 +1436,17 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob printf("Warning: resetting base station calibration data"); } + SurvivePose lighthousePose; FLT invRot[4]; - quatgetreciprocal(invRot, rotQuat); - - so->ctx->bsd[lh].Pose.Pos[0] = refinedEstimateGd.x; - so->ctx->bsd[lh].Pose.Pos[1] = refinedEstimateGd.y; - so->ctx->bsd[lh].Pose.Pos[2] = refinedEstimateGd.z; - so->ctx->bsd[lh].Pose.Rot[0] = invRot[0]; - so->ctx->bsd[lh].Pose.Rot[1] = invRot[1]; - so->ctx->bsd[lh].Pose.Rot[2] = invRot[2]; - so->ctx->bsd[lh].Pose.Rot[3] = invRot[3]; - so->ctx->bsd[lh].PositionSet = 1; + quatgetreciprocal(invRot, lighthousePose.Rot); + + lighthousePose.Pos[0] = refinedEstimateGd.x; + lighthousePose.Pos[1] = refinedEstimateGd.y; + lighthousePose.Pos[2] = refinedEstimateGd.z; + + if (so->ctx->lighthouseposeproc) { + so->ctx->lighthouseposeproc(so->ctx, lh, &lighthousePose); + } } diff --git a/src/survive.c b/src/survive.c index 5d0d35f..61bfc86 100755 --- a/src/survive.c +++ b/src/survive.c @@ -148,7 +148,7 @@ SurviveContext * survive_init_internal( int headless ) ctx->lightproc = survive_default_light_process; ctx->imuproc = survive_default_imu_process; ctx->angleproc = survive_default_angle_process; - + ctx->lighthouseposeproc = survive_default_lighthouse_pose_process; // initialize the button queue memset(&(ctx->buttonQueue), 0, sizeof(ctx->buttonQueue)); @@ -260,6 +260,14 @@ void survive_install_raw_pose_fn(SurviveContext * ctx, raw_pose_func fbp) else ctx->rawposeproc = survive_default_raw_pose_process; } + +void survive_install_lighthouse_pose_fn(SurviveContext *ctx, lighthouse_pose_func fbp) { + if (fbp) + ctx->lighthouseposeproc = fbp; + else + ctx->lighthouseposeproc = survive_default_lighthouse_pose_process; +} + int survive_add_object( SurviveContext * ctx, SurviveObject * obj ) { int oldct = ctx->objs_ct; diff --git a/src/survive_cal.c b/src/survive_cal.c index 696cce4..59cf68b 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -651,9 +651,6 @@ static void handle_calibration( struct SurviveCalData *cd ) fprintf( fobjp, "%f %f %f %f\n", objfromlh->Rot[0], objfromlh->Rot[1], objfromlh->Rot[2], objfromlh->Rot[3] ); if (ctx->bsd[lh].PositionSet) { - config_set_lighthouse(ctx->lh_config, &ctx->bsd[0], 0); - config_set_lighthouse(ctx->lh_config, &ctx->bsd[1], 1); - if (compute_reprojection_error) { FLT reproj_err = 0; size_t cnt = 0; @@ -690,7 +687,6 @@ static void handle_calibration( struct SurviveCalData *cd ) } } - config_save(ctx, "config.json"); } fclose( fobjp ); diff --git a/src/survive_process.c b/src/survive_process.c index d156d9e..420e771 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -2,6 +2,7 @@ //All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses. #include "survive_cal.h" +#include "survive_config.h" //XXX TODO: Once data is avialble in the context, use the stuff here to handle converting from time codes to //proper angles, then from there perform the rest of the solution. @@ -108,6 +109,18 @@ void survive_default_raw_pose_process(SurviveObject *so, uint8_t lighthouse, Sur } +void survive_default_lighthouse_pose_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *pose) { + if (pose) { + ctx->bsd[lighthouse].Pose = *pose; + ctx->bsd[lighthouse].PositionSet = 1; + } else { + ctx->bsd[lighthouse].PositionSet = 0; + } + + config_set_lighthouse(ctx->lh_config, &ctx->bsd[lighthouse], lighthouse); + config_save(ctx, "config.json"); +} + void survive_default_imu_process( SurviveObject * so, int mask, FLT * accelgyromag, uint32_t timecode, int id ) { if( so->PoserFn ) diff --git a/test.c b/test.c index bc9677a..d4b3d6f 100644 --- a/test.c +++ b/test.c @@ -84,6 +84,13 @@ void testprog_button_process(SurviveObject * so, uint8_t eventType, uint8_t butt } } +void testprog_lighthouse_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *pose) { + survive_default_lighthouse_pose_process(ctx, lighthouse, pose); + + printf("Lighthouse: [%1.1x][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, pose->Pos[0], + pose->Pos[1], pose->Pos[2], pose->Rot[0], pose->Rot[1], pose->Rot[2], pose->Rot[3]); +} + void testprog_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) { survive_default_raw_pose_process(so, lighthouse, pose); @@ -139,6 +146,7 @@ int main() survive_install_button_fn(ctx, testprog_button_process); survive_install_raw_pose_fn(ctx, testprog_raw_pose_process); survive_install_imu_fn(ctx, testprog_imu_process); + survive_install_lighthouse_pose_fn(ctx, testprog_lighthouse_process); if( !ctx ) { -- cgit v1.2.3 From 12f864db460dc6318694ec9e6d65f7740ae85749 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 11 Mar 2018 15:54:35 -0600 Subject: Preparing to add more to PoserData --- include/libsurvive/poser.h | 7 +++---- src/survive_cal.c | 2 +- src/survive_process.c | 16 +++++++++++----- 3 files changed, 15 insertions(+), 10 deletions(-) diff --git a/include/libsurvive/poser.h b/include/libsurvive/poser.h index 582590e..7e05184 100644 --- a/include/libsurvive/poser.h +++ b/include/libsurvive/poser.h @@ -20,12 +20,11 @@ typedef enum PoserType_t typedef struct { PoserType pt; - uint8_t data[0]; } PoserData; typedef struct { - PoserType pt; + PoserData hdr; uint8_t datamask; //0 = accel present, 1 = gyro present, 2 = mag present. FLT accel[3]; FLT gyro[3]; @@ -35,7 +34,7 @@ typedef struct typedef struct { - PoserType pt; + PoserData hdr; int sensor_id; int acode; //OOTX Code associated with this sweep. bit 1 indicates vertical(1) or horizontal(0) sweep int lh; //Lighthouse making this sweep @@ -46,7 +45,7 @@ typedef struct typedef struct { - PoserType pt; + PoserData hdr; //If "lengths[...]" < 0, means not a valid piece of sweep information. FLT lengths[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; diff --git a/src/survive_cal.c b/src/survive_cal.c index 59cf68b..26ed1ad 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -604,7 +604,7 @@ static void handle_calibration( struct SurviveCalData *cd ) { int i, j; PoserDataFullScene fsd; - fsd.pt = POSERDATA_FULL_SCENE; + fsd.hdr.pt = POSERDATA_FULL_SCENE; for( j = 0; j < NUM_LIGHTHOUSES; j++ ) for( i = 0; i < SENSORS_PER_OBJECT; i++ ) { diff --git a/src/survive_process.c b/src/survive_process.c index 420e771..e45a979 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -52,7 +52,10 @@ void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode if( so->PoserFn ) { PoserDataLight l = { - .pt = POSERDATA_LIGHT, + .hdr = + { + .pt = POSERDATA_LIGHT, + }, .sensor_id = sensor_id, .acode = acode, .timecode = timecode, @@ -126,11 +129,14 @@ void survive_default_imu_process( SurviveObject * so, int mask, FLT * accelgyrom if( so->PoserFn ) { PoserDataIMU imu = { - .pt = POSERDATA_IMU, + .hdr = + { + .pt = POSERDATA_IMU, + }, .datamask = mask, - .accel = { accelgyromag[0], accelgyromag[1], accelgyromag[2] }, - .gyro = { accelgyromag[3], accelgyromag[4], accelgyromag[5] }, - .mag = { accelgyromag[6], accelgyromag[7], accelgyromag[8] }, + .accel = {accelgyromag[0], accelgyromag[1], accelgyromag[2]}, + .gyro = {accelgyromag[3], accelgyromag[4], accelgyromag[5]}, + .mag = {accelgyromag[6], accelgyromag[7], accelgyromag[8]}, .timecode = timecode, }; so->PoserFn( so, (PoserData *)&imu ); -- cgit v1.2.3 From ee0fd0be34cd6715b704be534b7e97087905571f Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 11 Mar 2018 16:24:13 -0600 Subject: Redirected posers to used passed in callbacks --- Makefile | 2 +- include/libsurvive/poser.h | 9 +++++++++ src/poser.c | 20 ++++++++++++++++++++ src/poser_charlesslow.c | 4 +--- src/poser_turveytori.c | 30 +++++++++--------------------- src/survive_cal.c | 2 +- 6 files changed, 41 insertions(+), 26 deletions(-) create mode 100644 src/poser.c diff --git a/Makefile b/Makefile index 332f59d..b9cfbdc 100644 --- a/Makefile +++ b/Makefile @@ -37,7 +37,7 @@ REDISTS:=redist/json_helpers.o redist/linmath.o redist/jsmn.o redist/os_generic. ifeq ($(UNAME), Darwin) REDISTS:=$(REDISTS) redist/hid-osx.c endif -LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_default_devices.o src/survive_vive.o src/survive_playback.o src/survive_config.o src/survive_cal.o src/survive_reproject.o +LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_default_devices.o src/survive_vive.o src/survive_playback.o src/survive_config.o src/survive_cal.o src/survive_reproject.o src/poser.o #If you want to use HIDAPI on Linux. diff --git a/include/libsurvive/poser.h b/include/libsurvive/poser.h index 7e05184..57a8599 100644 --- a/include/libsurvive/poser.h +++ b/include/libsurvive/poser.h @@ -17,11 +17,20 @@ typedef enum PoserType_t POSERDATA_DISASSOCIATE, //If you get this, it doesn't contain data. It just tells you to please disassociate from the current SurviveObject and delete your poserdata. } PoserType; +typedef void (*poser_raw_pose_func)(SurviveObject *so, uint8_t lighthouse, FLT *pose, void *user); +typedef void (*poser_lighthouse_pose_func)(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose, void *user); + typedef struct { PoserType pt; + poser_raw_pose_func rawposeproc; + poser_lighthouse_pose_func lighthouseposeproc; + void *userdata; } PoserData; +void PoserData_poser_raw_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse, FLT *pose); +void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse, SurvivePose *pose); + typedef struct { PoserData hdr; diff --git a/src/poser.c b/src/poser.c new file mode 100644 index 0000000..73fc7e8 --- /dev/null +++ b/src/poser.c @@ -0,0 +1,20 @@ +#include + +#include "poser.h" +#include "survive_internal.h" + +void PoserData_poser_raw_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse, FLT *pose) { + if (poser_data->rawposeproc) { + poser_data->rawposeproc(so, lighthouse, pose, poser_data->userdata); + } else { + so->ctx->rawposeproc(so, lighthouse, pose); + } +} + +void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) { + if (poser_data->lighthouseposeproc) { + poser_data->lighthouseposeproc(so, lighthouse, pose, poser_data->userdata); + } else { + so->ctx->lighthouseposeproc(so->ctx, lighthouse, pose); + } +} diff --git a/src/poser_charlesslow.c b/src/poser_charlesslow.c index 30fc677..0839d59 100644 --- a/src/poser_charlesslow.c +++ b/src/poser_charlesslow.c @@ -174,9 +174,7 @@ int PoserCharlesSlow( SurviveObject * so, PoserData * pd ) quatrotateabout(tmp, lighthousePose.Rot, rt); memcpy(lighthousePose.Rot, tmp, sizeof(FLT) * 4); - if (ctx->lighthouseposeproc) { - ctx->lighthouseposeproc(ctx, lh, &lighthousePose); - } + PoserData_lighthouse_pose_func(pd, so, lh, &lighthousePose); #define ALT_COORDS #ifdef ALT_COORDS diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index ddc4ad2..db46b09 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1234,9 +1234,8 @@ void SolveForRotationQuat(FLT rotOut[4], TrackedObject *obj, Point lh) } - -static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *obj, SurviveObject *so, char doLogOutput,const int lh,const int setLhCalibration) -{ +static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *obj, SurviveObject *so, PoserData *pd, + char doLogOutput, const int lh, const int setLhCalibration) { ToriData *toriData = so->PoserData; //printf("Solving for Lighthouse\n"); @@ -1444,9 +1443,7 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob lighthousePose.Pos[1] = refinedEstimateGd.y; lighthousePose.Pos[2] = refinedEstimateGd.z; - if (so->ctx->lighthouseposeproc) { - so->ctx->lighthouseposeproc(so->ctx, lh, &lighthousePose); - } + PoserData_lighthouse_pose_func(pd, so, lh, &lighthousePose); } @@ -1531,16 +1528,7 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob return refinedEstimateGd; } - - - - - - - -static void QuickPose(SurviveObject *so, int lh) -{ - +static void QuickPose(SurviveObject *so, PoserData *pd, int lh) { ToriData * td = so->PoserData; @@ -1634,7 +1622,7 @@ static void QuickPose(SurviveObject *so, int lh) // SolveForLighthouse(pos, quat, to, so, 0, lh, 0); //} - SolveForLighthouse(&pose.Pos[0], &pose.Rot[0], to, so, 0, lh, 0); + SolveForLighthouse(&pose.Pos[0], &pose.Rot[0], to, so, pd, 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]); if (so->ctx->rawposeproc) @@ -1717,7 +1705,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) // let's just do this occasionally for now... if (counter % 4 == 0) - QuickPose(so, 0); + QuickPose(so, poserData, 0); } if (1 == l->lh && axis) // only once per full cycle... { @@ -1727,7 +1715,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) // let's just do this occasionally for now... if (counter % 4 == 0) - QuickPose(so, 1); + QuickPose(so, poserData, 1); } // axis changed, time to increment the circular buffer index. td->angleIndex[l->lh][axis]++; @@ -1802,7 +1790,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) FLT pos[3], quat[4]; - SolveForLighthouse(pos, quat, to, so, 0, 0, 1); + SolveForLighthouse(pos, quat, to, so, poserData, 0, 0, 1); } { int sensorCount = 0; @@ -1838,7 +1826,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) FLT pos[3], quat[4]; - SolveForLighthouse(pos, quat, to, so, 0, 1, 1); + SolveForLighthouse(pos, quat, to, so, poserData, 0, 1, 1); } diff --git a/src/survive_cal.c b/src/survive_cal.c index 26ed1ad..fb3b240 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -603,7 +603,7 @@ static void handle_calibration( struct SurviveCalData *cd ) for( obj = 0; obj < cd->numPoseObjects; obj++ ) { int i, j; - PoserDataFullScene fsd; + PoserDataFullScene fsd = {}; fsd.hdr.pt = POSERDATA_FULL_SCENE; for( j = 0; j < NUM_LIGHTHOUSES; j++ ) for( i = 0; i < SENSORS_PER_OBJECT; i++ ) -- cgit v1.2.3