diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/poser_imu.c | 33 | ||||
-rw-r--r-- | src/poser_sba.c | 19 | ||||
-rw-r--r-- | src/survive.c | 5 | ||||
-rw-r--r-- | src/survive_default_devices.c | 9 | ||||
-rw-r--r-- | src/survive_disambiguator.c | 16 | ||||
-rw-r--r-- | src/survive_imu.c | 118 | ||||
-rw-r--r-- | src/survive_playback.c | 4 | ||||
-rw-r--r-- | src/survive_process.c | 22 | ||||
-rw-r--r-- | src/survive_reproject.c | 1 | ||||
-rwxr-xr-x | src/survive_vive.c | 39 |
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 ); } |