aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/poser_imu.c33
-rw-r--r--src/poser_sba.c19
-rw-r--r--src/survive.c5
-rw-r--r--src/survive_default_devices.c9
-rw-r--r--src/survive_disambiguator.c16
-rw-r--r--src/survive_imu.c118
-rw-r--r--src/survive_playback.c4
-rw-r--r--src/survive_process.c22
-rw-r--r--src/survive_reproject.c1
-rwxr-xr-xsrc/survive_vive.c39
10 files changed, 220 insertions, 46 deletions
diff --git a/src/poser_imu.c b/src/poser_imu.c
new file mode 100644
index 0000000..02ff8e9
--- /dev/null
+++ b/src/poser_imu.c
@@ -0,0 +1,33 @@
+#include <survive.h>
+#include <survive_imu.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+
+int PoserIMU(SurviveObject *so, PoserData *pd) {
+ PoserType pt = pd->pt;
+ SurviveContext *ctx = so->ctx;
+ SurviveIMUTracker *dd = so->PoserData;
+
+ if (!dd) {
+ so->PoserData = dd = malloc(sizeof(SurviveIMUTracker));
+ *dd = (SurviveIMUTracker){};
+ }
+
+ switch (pt) {
+ case POSERDATA_IMU: {
+ PoserDataIMU *imu = (PoserDataIMU *)pd;
+
+ survive_imu_tracker_integrate(so, dd, imu);
+
+ PoserData_poser_raw_pose_func(pd, so, -1, &dd->pose);
+
+ // if(magnitude3d(dd->pose.Pos) > 1)
+ // SV_ERROR("IMU drift");
+ return 0;
+ }
+ }
+ return -1;
+}
+
+REGISTER_LINKTIME(PoserIMU);
diff --git a/src/poser_sba.c b/src/poser_sba.c
index bd7d520..1dbc820 100644
--- a/src/poser_sba.c
+++ b/src/poser_sba.c
@@ -3,11 +3,12 @@
#define USE_DOUBLE
#endif
-#include <sba/sba.h>
#include <malloc.h>
+#include <sba/sba.h>
#include "poser.h"
#include <survive.h>
+#include <survive_imu.h>
#include "assert.h"
#include "linmath.h"
@@ -47,6 +48,9 @@ typedef struct SBAData {
int required_meas;
+ SurviveIMUTracker tracker;
+ bool useIMU;
+
SurviveObject *so;
} SBAData;
@@ -421,6 +425,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) {
FLT error = -1;
if (d->last_lh != lightData->lh || d->last_acode != lightData->acode) {
error = run_sba_find_3d_structure(d, lightData, scene, 100, .5);
+
d->last_lh = lightData->lh;
d->last_acode = lightData->acode;
}
@@ -429,6 +434,9 @@ int PoserSBA(SurviveObject *so, PoserData *pd) {
if (d->failures_to_reset_cntr > 0)
d->failures_to_reset_cntr--;
} else {
+ if (d->useIMU) {
+ survive_imu_tracker_set_pose(&d->tracker, lightData->timecode, &so->OutPose);
+ }
if (d->successes_to_reset_cntr > 0)
d->successes_to_reset_cntr--;
}
@@ -442,6 +450,15 @@ int PoserSBA(SurviveObject *so, PoserData *pd) {
// std::cerr << "Average reproj error: " << error << std::endl;
return 0;
}
+ case POSERDATA_IMU: {
+
+ PoserDataIMU * imu = (PoserDataIMU*)pd;
+ if (ctx->calptr && ctx->calptr->stage < 5) {
+ } else if(d->useIMU){
+ survive_imu_tracker_integrate(so, &d->tracker, imu);
+ PoserData_poser_raw_pose_func(pd, so, 1, &d->tracker.pose);
+ }
+ } // INTENTIONAL FALLTHROUGH
default: {
const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP");
PoserCB driver = (PoserCB)GetDriver(subposer);
diff --git a/src/survive.c b/src/survive.c
index 2a7aad1..1a782a2 100644
--- a/src/survive.c
+++ b/src/survive.c
@@ -86,9 +86,8 @@ void survive_verify_FLT_size(uint32_t user_size) {
if (sizeof(FLT) != user_size) {
fprintf(stderr, "FLT type incompatible; the shared library libsurvive has FLT size %lu vs user program %u\n",
(unsigned long)sizeof(FLT), user_size);
- fprintf(stderr,
- "Add '#define FLT %s' before including survive.h or recompile the shared library with the "
- "appropriate flag. \n",
+ 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);
}
diff --git a/src/survive_default_devices.c b/src/survive_default_devices.c
index 6b65752..2e47b9e 100644
--- a/src/survive_default_devices.c
+++ b/src/survive_default_devices.c
@@ -144,11 +144,10 @@ int survive_load_htc_config_format(SurviveObject *so, char *ct0conf, int len) {
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;
+ const FLT bias_units = 1. / 1000.; // I deeply suspect bias is in milligravities -JB
+ so->acc_bias[0] *= bias_units;
+ so->acc_bias[1] *= bias_units;
+ so->acc_bias[2] *= bias_units;
}
}
if (jsoneq(ct0conf, tk, "acc_scale") == 0) {
diff --git a/src/survive_disambiguator.c b/src/survive_disambiguator.c
index 6c19475..73e61a8 100644
--- a/src/survive_disambiguator.c
+++ b/src/survive_disambiguator.c
@@ -1,19 +1,21 @@
#include "survive.h"
+#include "survive_playback.h"
#include <os_generic.h>
#include <stdio.h>
-#include "survive_playback.h"
//#define LOG_LIGHTDATA
-void handle_lightcap(SurviveObject *so, LightcapElement *le)
-{
- survive_recording_lightcap(so, le);
+void handle_lightcap(SurviveObject *so, LightcapElement *le) {
+ survive_recording_lightcap(so, le);
#ifdef LOG_LIGHTDATA
- static FILE * flog;
+ static FILE *flog;
static double start = 0;
- if( !flog ) { flog = fopen( "lightcap.txt", "wb" ); start = OGGetAbsoluteTime(); }
- fprintf( flog, "%.6f %2d %4d %9d\n", OGGetAbsoluteTime()-start, le->sensor_id, le->length, le->timestamp );
+ if (!flog) {
+ flog = fopen("lightcap.txt", "wb");
+ start = OGGetAbsoluteTime();
+ }
+ fprintf(flog, "%.6f %2d %4d %9d\n", OGGetAbsoluteTime() - start, le->sensor_id, le->length, le->timestamp);
#endif
so->ctx->lightcapfunction(so, le);
}
diff --git a/src/survive_imu.c b/src/survive_imu.c
new file mode 100644
index 0000000..297bbac
--- /dev/null
+++ b/src/survive_imu.c
@@ -0,0 +1,118 @@
+#include "survive_imu.h"
+#include "linmath.h"
+#include "survive_internal.h"
+#include <survive_imu.h>
+
+void survive_imu_tracker_set_pose(SurviveIMUTracker *tracker, uint32_t timecode, SurvivePose *pose) {
+ tracker->pose = *pose;
+
+ for (int i = 0; i < 3; i++)
+ tracker->current_velocity[i] =
+ (pose->Pos[i] - tracker->lastGT.Pos[i]) / (timecode - tracker->lastGTTime) * 48000000.;
+
+ tracker->lastGTTime = timecode;
+ tracker->lastGT = *pose;
+}
+
+static const int imu_calibration_iterations = 100;
+
+static void RotateAccel(LinmathVec3d rAcc, const SurvivePose *pose, const LinmathVec3d accel) {
+ quatrotatevector(rAcc, pose->Rot, accel);
+ LinmathVec3d G = {0, 0, -1};
+ add3d(rAcc, rAcc, G);
+ scale3d(rAcc, rAcc, 9.8);
+}
+static SurvivePose iterate_position(const SurvivePose *pose, const LinmathVec3d vel, double time_diff,
+ const PoserDataIMU *pIMU) {
+ SurvivePose result = *pose;
+
+ FLT acc_mul = time_diff * time_diff / 2;
+ LinmathVec3d rAcc = {0};
+ RotateAccel(rAcc, pose, pIMU->accel);
+
+ //fprintf(stderr, "r %f %f %f %f\n", pIMU->accel[0], pIMU->accel[1], pIMU->accel[2], quatmagnitude(pIMU->accel));
+ //fprintf(stderr, "i %f %f %f %f\n", rAcc[0], rAcc[1], rAcc[2], quatmagnitude(rAcc));
+
+ scale3d(rAcc, rAcc, acc_mul);
+
+ LinmathVec3d gyro;
+
+ for (int i = 0; i < 3; i++) {
+ result.Pos[i] += time_diff * vel[i] + rAcc[i];
+ gyro[i] = time_diff / 2 * pIMU->gyro[i];
+ }
+
+ LinmathEulerAngle curr, next;
+ quattoeuler(curr, pose->Rot);
+ add3d(next, curr, gyro);
+ quatfromeuler(result.Rot, next);
+
+ return result;
+}
+
+static void iterate_velocity(LinmathVec3d result, const SurvivePose *pose, const LinmathVec3d vel, double time_diff,
+ PoserDataIMU *pIMU) {
+ scale3d(result, vel, 1.);
+ LinmathVec3d rAcc = {0};
+ RotateAccel(rAcc, pose, pIMU->accel);
+ scale3d(rAcc, rAcc, time_diff);
+ add3d(result, result, rAcc);
+}
+
+void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker, PoserDataIMU *data) {
+ if (tracker->last_data.timecode == 0) {
+ if (tracker->last_data.datamask == imu_calibration_iterations) {
+ tracker->last_data = *data;
+ tracker->pose.Rot[0] = 1.;
+
+ const FLT up[3] = {0, 0, 1};
+ quatfrom2vectors(tracker->pose.Rot, tracker->updir, up);
+ return;
+ }
+
+ tracker->last_data.datamask++;
+
+ tracker->updir[0] += data->accel[0] / imu_calibration_iterations;
+ tracker->updir[1] += data->accel[1] / imu_calibration_iterations;
+ tracker->updir[2] += data->accel[2] / imu_calibration_iterations;
+ return;
+ }
+
+ for (int i = 0; i < 3; i++) {
+ tracker->updir[i] = data->accel[i] * .10 + tracker->updir[i] * .90;
+ }
+
+ const FLT up[3] = {0, 0, 1};
+ LinmathQuat upRot, wouldbeUp;
+ LinmathVec3d rup;
+ quatrotatevector(rup, tracker->pose.Rot, up);
+ quatfrom2vectors(upRot, rup, data->accel);
+
+ quatrotateabout(wouldbeUp, upRot, tracker->pose.Rot);
+ quatslerp(tracker->pose.Rot, tracker->pose.Rot, wouldbeUp, .1);
+
+ FLT pose_up[3] = {0, 0, 1};
+ quatrotatevector(pose_up, tracker->pose.Rot, tracker->updir);
+
+ FLT time_diff = (data->timecode - tracker->last_data.timecode) / (FLT)so->timebase_hz;
+
+ SurvivePose t_next = iterate_position(&tracker->pose, tracker->current_velocity, time_diff, data);
+
+ LinmathVec3d v_next;
+ iterate_velocity(v_next, &tracker->pose, tracker->current_velocity, time_diff, data);
+
+ tracker->pose = t_next;
+
+ //fprintf(stderr, "%f %f %f\n", tracker->current_velocity[0], tracker->current_velocity[1],
+ //tracker->current_velocity[2]);
+
+ scale3d(tracker->current_velocity, v_next, 1);
+
+ tracker->last_data = *data;
+
+ FLT tmp[3];
+ ApplyPoseToPoint(tmp, &tracker->pose, up);
+
+ printf("[%f, %f, %f] [%f, %f, %f]\n", tracker->pose.Pos[0], tracker->pose.Pos[1], tracker->pose.Pos[2], tmp[0],
+ tmp[1], tmp[2]);
+}
diff --git a/src/survive_playback.c b/src/survive_playback.c
index 6789a66..d5d1c08 100644
--- a/src/survive_playback.c
+++ b/src/survive_playback.c
@@ -17,8 +17,8 @@
typedef long ssize_t;
#define SSIZE_MAX LONG_MAX
-ssize_t getdelim(char ** lineptr, size_t * n, int delimiter, FILE *stream);
-ssize_t getline(char **lineptr, size_t * n, FILE *stream);
+ssize_t getdelim(char **lineptr, size_t *n, int delimiter, FILE *stream);
+ssize_t getline(char **lineptr, size_t *n, FILE *stream);
#endif
typedef struct SurviveRecordingData {
diff --git a/src/survive_process.c b/src/survive_process.c
index 62459f2..e05e809 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -49,20 +49,20 @@ void survive_default_light_process( SurviveObject * so, int sensor_id, int acode
//Need to now do angle correction.
static int use_bsd_cal = -1;
- if(use_bsd_cal == -1) {
- use_bsd_cal = survive_configi(ctx, "use-bsd-cal", SC_GET, 1);
- if (use_bsd_cal == 0) {
- SV_INFO("Not using BSD calibration values");
- }
+ if (use_bsd_cal == -1) {
+ use_bsd_cal = survive_configi(ctx, "use-bsd-cal", SC_GET, 1);
+ if (use_bsd_cal == 0) {
+ SV_INFO("Not using BSD calibration values");
+ }
}
- if(use_bsd_cal) {
- BaseStationData * bsd = &ctx->bsd[base_station];
+ if (use_bsd_cal) {
+ BaseStationData *bsd = &ctx->bsd[base_station];
- // XXX TODO: This seriously needs to be worked on. See: https://github.com/cnlohr/libsurvive/issues/18
- // angle += (use_bsd_cal == 2 ? -1 : 1) * bsd->fcal.phase[axis];
- // angle += bsd->fcaltilt[axis] * predicted_angle(axis1);
+ // XXX TODO: This seriously needs to be worked on. See: https://github.com/cnlohr/libsurvive/issues/18
+ // angle += (use_bsd_cal == 2 ? -1 : 1) * bsd->fcal.phase[axis];
+ // angle += bsd->fcaltilt[axis] * predicted_angle(axis1);
- //TODO!!!
+ // TODO!!!
}
FLT length_sec = length / (FLT)so->timebase_hz;
diff --git a/src/survive_reproject.c b/src/survive_reproject.c
index 0eaceb2..d6ba70b 100644
--- a/src/survive_reproject.c
+++ b/src/survive_reproject.c
@@ -43,7 +43,6 @@ void survive_reproject_from_pose_with_bsd(const BaseStationData *bsd, const surv
if (f & SVCal_Gib)
out[axis] -= config->gib_scale * sin(gibPhase[axis] + ang[axis]) * gibMag[axis];
}
-
}
void survive_apply_bsd_calibration_by_config(SurviveContext *ctx, int lh, struct survive_calibration_config *config,
diff --git a/src/survive_vive.c b/src/survive_vive.c
index 3c60b2a..53f6d42 100755
--- a/src/survive_vive.c
+++ b/src/survive_vive.c
@@ -1485,26 +1485,33 @@ void survive_data_cb( SurviveUSBInterface * si )
obj->oldcode = code;
//XXX XXX BIG TODO!!! Actually recal gyro data.
- 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);
- agm[1]*=(float)(1./8192.0);
- agm[2]*=(float)(1./8192.0);
- calibrate_acc(obj, agm);
+ FLT agm[9] = {acceldata[0].v,
+ acceldata[1].v,
+ acceldata[2].v,
+ acceldata[3].v,
+ acceldata[4].v,
+ acceldata[5].v,
+ 0,
+ 0,
+ 0};
//1G for accelerometer, from MPU6500 datasheet
//this can change if the firmware changes the sensitivity.
+ // When coming off of USB, these values are in units of .5g -JB
+ agm[0] *= (float)(2. / 8192.0);
+ agm[1] *= (float)(2. / 8192.0);
+ agm[2] *= (float)(2. / 8192.0);
+ calibrate_acc(obj, agm);
-
- agm[3]*=(float)((1./32.768)*(3.14159/180.));
- agm[4]*=(float)((1./32.768)*(3.14159/180.));
- agm[5]*=(float)((1./32.768)*(3.14159/180.));
- calibrate_gyro(obj, agm+3);
-
- //65.5 deg/s for gyroscope, from MPU6500 datasheet
- //1000 deg/s for gyroscope, from MPU6500 datasheet
+ // From datasheet, can be 250, 500, 1000, 2000 deg/s range over 16 bits
+ // FLT deg_per_sec = 250;
+ // FLT conv = (float)((1./deg_per_sec)*(3.14159/180.)) / 8192.;
+ FLT DEGREES_TO_RADS = 3.14159 / 180.;
+ FLT conv = 1. / 10. * DEGREES_TO_RADS;
+ // calibrate_gyro(obj, agm+3);
+ agm[3] *= conv;
+ agm[4] *= conv;
+ agm[5] *= conv;
ctx->imuproc( obj, 3, agm, timecode, code );
}