aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorJustin Berger <j.david.berger@gmail.com>2018-03-11 15:31:28 -0600
committerJustin Berger <j.david.berger@gmail.com>2018-03-12 09:52:50 -0600
commit95eab1b8938e0033a0aa0d10d6ec53d7d2782907 (patch)
treee92f9abf0287fea8c91abadf8a095318507a8885
parent7fadeeff29074520224be385ecd6c50271a07220 (diff)
downloadlibsurvive-95eab1b8938e0033a0aa0d10d6ec53d7d2782907.tar.gz
libsurvive-95eab1b8938e0033a0aa0d10d6ec53d7d2782907.tar.bz2
Added callback for lighthouse found; example in test.c
-rw-r--r--include/libsurvive/survive.h3
-rw-r--r--include/libsurvive/survive_types.h1
-rw-r--r--src/poser_charlesslow.c20
-rw-r--r--src/poser_turveytori.c20
-rwxr-xr-xsrc/survive.c10
-rwxr-xr-xsrc/survive_cal.c4
-rw-r--r--src/survive_process.c13
-rw-r--r--test.c8
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 )
{