aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--Makefile2
-rw-r--r--include/libsurvive/poser.h16
-rw-r--r--include/libsurvive/survive.h3
-rw-r--r--include/libsurvive/survive_types.h1
-rw-r--r--src/poser.c20
-rw-r--r--src/poser_charlesslow.c18
-rw-r--r--src/poser_turveytori.c44
-rwxr-xr-xsrc/survive.c10
-rwxr-xr-xsrc/survive_cal.c8
-rw-r--r--src/survive_process.c29
-rw-r--r--test.c8
11 files changed, 106 insertions, 53 deletions
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 582590e..57a8599 100644
--- a/include/libsurvive/poser.h
+++ b/include/libsurvive/poser.h
@@ -17,15 +17,23 @@ 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;
- uint8_t data[0];
+ 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
{
- PoserType pt;
+ PoserData hdr;
uint8_t datamask; //0 = accel present, 1 = gyro present, 2 = mag present.
FLT accel[3];
FLT gyro[3];
@@ -35,7 +43,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 +54,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/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.c b/src/poser.c
new file mode 100644
index 0000000..73fc7e8
--- /dev/null
+++ b/src/poser.c
@@ -0,0 +1,20 @@
+#include <stdint.h>
+
+#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 96442b3..0839d59 100644
--- a/src/poser_charlesslow.c
+++ b/src/poser_charlesslow.c
@@ -165,9 +165,16 @@ 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);
+
+ PoserData_lighthouse_pose_func(pd, so, lh, &lighthousePose);
#define ALT_COORDS
#ifdef ALT_COORDS
@@ -189,11 +196,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..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");
@@ -1436,17 +1435,15 @@ 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;
+
+ 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.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..fb3b240 100755
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -603,8 +603,8 @@ static void handle_calibration( struct SurviveCalData *cd )
for( obj = 0; obj < cd->numPoseObjects; obj++ )
{
int i, j;
- PoserDataFullScene fsd;
- fsd.pt = POSERDATA_FULL_SCENE;
+ PoserDataFullScene fsd = {};
+ fsd.hdr.pt = POSERDATA_FULL_SCENE;
for( j = 0; j < NUM_LIGHTHOUSES; j++ )
for( i = 0; i < SENSORS_PER_OBJECT; i++ )
{
@@ -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..e45a979 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.
@@ -51,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,
@@ -108,16 +112,31 @@ 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 )
{
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 );
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 )
{