diff options
-rw-r--r-- | Makefile | 3 | ||||
-rw-r--r-- | include/libsurvive/survive.h | 2 | ||||
-rw-r--r-- | include/libsurvive/survive_imu.h | 36 | ||||
-rw-r--r-- | include/libsurvive/survive_types.h | 3 | ||||
-rw-r--r-- | redist/linmath.h | 3 | ||||
-rw-r--r-- | redist/symbol_enumerator.c | 12 | ||||
-rw-r--r-- | simple_pose_test.c | 6 | ||||
-rw-r--r-- | src/poser.c | 2 | ||||
-rw-r--r-- | src/poser_charlesrefine.c | 335 | ||||
-rw-r--r-- | src/poser_charlesslow.c | 4 | ||||
-rw-r--r-- | src/poser_imu.c | 33 | ||||
-rw-r--r-- | src/poser_sba.c | 38 | ||||
-rw-r--r-- | src/poser_turveytori.c | 3 | ||||
-rw-r--r-- | src/survive.c | 8 | ||||
-rw-r--r-- | src/survive_default_devices.c | 9 | ||||
-rw-r--r-- | src/survive_disambiguator.c | 16 | ||||
-rw-r--r-- | src/survive_imu.c | 220 | ||||
-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 | ||||
-rw-r--r-- | tools/viz/survive_viewer.js | 15 | ||||
-rw-r--r-- | winbuild/getdelim.c | 36 |
23 files changed, 569 insertions, 281 deletions
@@ -39,7 +39,8 @@ REDISTS:=redist/json_helpers.o redist/linmath.o redist/jsmn.o redist/minimal_ope ifeq ($(UNAME), Darwin) REDISTS:=$(REDISTS) redist/hid-osx.c endif -LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_charlesbiguator.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 src/epnp/epnp.o src/survive_sensor_activations.o src/survive_turveybiguator.o src/survive_disambiguator.o src/survive_statebased_disambiguator.o src/poser_charlesrefine.o + +LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_charlesbiguator.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 src/epnp/epnp.o src/survive_sensor_activations.o src/survive_turveybiguator.o src/survive_disambiguator.o src/survive_statebased_disambiguator.o src/poser_charlesrefine.o src/survive_imu.o src/poser_imu.o #If you want to use HIDAPI on Linux. #CFLAGS:=$(CFLAGS) -DHIDAPI diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 0558dc8..65343b7 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -115,7 +115,7 @@ struct SurviveObject { haptic_func haptic; SurviveSensorActivations activations; - void* user_ptr; + void *user_ptr; // Debug int tsl; }; diff --git a/include/libsurvive/survive_imu.h b/include/libsurvive/survive_imu.h new file mode 100644 index 0000000..124ad7e --- /dev/null +++ b/include/libsurvive/survive_imu.h @@ -0,0 +1,36 @@ +#ifndef _SURVIVE_IMU_H +#define _SURVIVE_IMU_H + +#include "poser.h" +#include "survive_types.h" +#include <stdint.h> + +#ifdef __cplusplus +extern "C" { +#endif + +struct SurviveIMUTracker_p; + +typedef struct { + FLT updir[3]; + FLT accel_scale_bias; + + LinmathVec3d current_velocity; // Velocity in world frame + PoserDataIMU last_data; + SurvivePose pose; + + SurvivePose lastGT; + uint32_t lastGTTime; + + float integralFBx, integralFBy, integralFBz; // integral error terms scaled by Ki + +} SurviveIMUTracker; + +void survive_imu_tracker_set_pose(SurviveIMUTracker *tracker, uint32_t timecode, SurvivePose *pose); +void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker, PoserDataIMU *data); + +#ifdef __cplusplus +}; +#endif + +#endif diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h index edce3e9..367c391 100644 --- a/include/libsurvive/survive_types.h +++ b/include/libsurvive/survive_types.h @@ -50,7 +50,8 @@ 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 (*pose_func)(SurviveObject *so, uint32_t timecode, SurvivePose *pose); -typedef void (*lighthouse_pose_func)(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lighthouse_pose, SurvivePose *object_pose); +typedef void (*lighthouse_pose_func)(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lighthouse_pose, + SurvivePose *object_pose); // For lightcap, etc. Don't change this structure at all. Regular vive is dependent on it being exactly as-is. // When you write drivers, you can use this to send survive lightcap data. diff --git a/redist/linmath.h b/redist/linmath.h index 094c230..e268e96 100644 --- a/redist/linmath.h +++ b/redist/linmath.h @@ -7,7 +7,6 @@ extern "C" { #endif - #ifdef _WIN32 #define LINMATH_EXPORT __declspec(dllexport) #else @@ -116,7 +115,7 @@ LINMATH_EXPORT void quatgetreciprocal(LinmathQuat qout, const LinmathQuat qin); LINMATH_EXPORT void quatsub(LinmathQuat qout, const LinmathQuat a, const LinmathQuat b); LINMATH_EXPORT void quatadd(LinmathQuat qout, const LinmathQuat a, const LinmathQuat b); LINMATH_EXPORT void quatrotateabout(LinmathQuat qout, const LinmathQuat a, - const LinmathQuat b); // same as quat multiply, not piecewise multiply. + const LinmathQuat b); // same as quat multiply, not piecewise multiply. LINMATH_EXPORT void quatscale(LinmathQuat qout, const LinmathQuat qin, FLT s); FLT quatinnerproduct(const LinmathQuat qa, const LinmathQuat qb); LINMATH_EXPORT void quatouterproduct(FLT *outvec3, LinmathQuat qa, LinmathQuat qb); diff --git a/redist/symbol_enumerator.c b/redist/symbol_enumerator.c index 58bbfaa..31bb68e 100644 --- a/redist/symbol_enumerator.c +++ b/redist/symbol_enumerator.c @@ -60,18 +60,14 @@ BOOL WINAPI SymCleanup( HANDLE hProcess ); -BOOL mycb( - PSYMBOL_INFO pSymInfo, - ULONG SymbolSize, - PVOID UserContext - ){ - SymEnumeratorCallback cb = (SymEnumeratorCallback)UserContext; - return !cb( "", &pSymInfo->Name[0], (void*)pSymInfo->Address, (long) pSymInfo->Size ); +BOOL mycb(PSYMBOL_INFO pSymInfo, ULONG SymbolSize, PVOID UserContext) { + SymEnumeratorCallback cb = (SymEnumeratorCallback)UserContext; + return !cb("", &pSymInfo->Name[0], (void *)pSymInfo->Address, (long)pSymInfo->Size); } int EnumerateSymbols( SymEnumeratorCallback cb ) { - HANDLE proc = GetCurrentProcess(); + HANDLE proc = GetCurrentProcess(); if( !SymInitialize( proc, 0, 1 ) ) return -1; if( !SymEnumSymbols( proc, 0, "*!*", &mycb, (void*)cb ) ) { diff --git a/simple_pose_test.c b/simple_pose_test.c index d28e78d..06a9901 100644 --- a/simple_pose_test.c +++ b/simple_pose_test.c @@ -43,9 +43,9 @@ FLT hpos[3]; FLT hpos2[3]; void testprog_raw_pose_process(SurviveObject *so, uint32_t timecode, SurvivePose *pose) { - survive_default_raw_pose_process(so, timecode, pose ); + survive_default_raw_pose_process(so, timecode, pose); - if( strcmp( so->codename, "HMD" ) != 0 ) + if (strcmp(so->codename, "HMD") != 0) return; // print the pose; @@ -168,7 +168,7 @@ int main( int argc, char ** argv ) survive_install_pose_fn(ctx, testprog_raw_pose_process); //survive_install_angle_fn(ctx, testprog_angle_process ); -#if 0 //Don't reset poses +#if 0 // Don't reset poses ctx->bsd[0].PositionSet = ctx->bsd[1].PositionSet = 1; int i; for( i = 0; i < 2; i++ ) diff --git a/src/poser.c b/src/poser.c index 20334f7..499e0ff 100644 --- a/src/poser.c +++ b/src/poser.c @@ -14,7 +14,7 @@ static uint32_t PoserData_timecode(PoserData *poser_data) { return lightData->timecode; } case POSERDATA_FULL_SCENE: { - PoserDataFullScene* pdfs = (PoserDataFullScene *)(poser_data); + PoserDataFullScene *pdfs = (PoserDataFullScene *)(poser_data); return -1; } case POSERDATA_IMU: { diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index f9c60e2..388ba77 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -1,4 +1,4 @@ -//Driver works, but you _must_ start it near the origin looking in +Z. +// Driver works, but you _must_ start it near the origin looking in +Z. #include <poser.h> #include <survive.h> @@ -7,18 +7,16 @@ #include "epnp/epnp.h" #include "linmath.h" #include <math.h> +#include <stdint.h> #include <stdio.h> #include <string.h> -#include <stdint.h> #define MAX_PT_PER_SWEEP 32 - -typedef struct -{ +typedef struct { int sweepaxis; int sweeplh; - FLT normal_at_errors[MAX_PT_PER_SWEEP][3]; //Value is actually normalized, not just normal to sweep plane. + FLT normal_at_errors[MAX_PT_PER_SWEEP][3]; // Value is actually normalized, not just normal to sweep plane. FLT quantity_errors[MAX_PT_PER_SWEEP]; FLT angles_at_pts[MAX_PT_PER_SWEEP]; SurvivePose object_pose_at_hit[MAX_PT_PER_SWEEP]; @@ -26,11 +24,10 @@ typedef struct int ptsweep; } CharlesPoserData; - - int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { - CharlesPoserData * dd = so->PoserData; - if( !dd ) so->PoserData = dd = calloc( sizeof(CharlesPoserData), 1 ); + CharlesPoserData *dd = so->PoserData; + if (!dd) + so->PoserData = dd = calloc(sizeof(CharlesPoserData), 1); SurviveSensorActivations *scene = &so->activations; switch (pd->pt) { @@ -38,18 +35,17 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { // Really should use this... PoserDataIMU *imuData = (PoserDataIMU *)pd; - - //TODO: Actually do Madgwick's algorithm - LinmathQuat applymotion; - const SurvivePose * object_pose = &so->OutPose; + // TODO: Actually do Madgwick's algorithm + LinmathQuat applymotion; + const SurvivePose *object_pose = &so->OutPose; imuData->gyro[0] *= -0.0005; imuData->gyro[1] *= -0.0005; imuData->gyro[2] *= 0.0005; - quatfromeuler( applymotion, imuData->gyro ); - //printf( "%f %f %f\n", imuData->gyro [0], imuData->gyro [1], imuData->gyro [2] ); + quatfromeuler(applymotion, imuData->gyro); + // printf( "%f %f %f\n", imuData->gyro [0], imuData->gyro [1], imuData->gyro [2] ); SurvivePose object_pose_out; - quatrotateabout(object_pose_out.Rot, object_pose->Rot, applymotion ); - copy3d( object_pose_out.Pos, object_pose->Pos ); + quatrotateabout(object_pose_out.Rot, object_pose->Rot, applymotion); + copy3d(object_pose_out.Pos, object_pose->Pos); PoserData_poser_pose_func(pd, so, &object_pose_out); return 0; @@ -59,70 +55,67 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { PoserDataLight *ld = (PoserDataLight *)pd; int lhid = ld->lh; int senid = ld->sensor_id; - BaseStationData * bsd = &so->ctx->bsd[ld->lh]; - if( !bsd->PositionSet ) break; - SurvivePose * lhp = &bsd->Pose; + BaseStationData *bsd = &so->ctx->bsd[ld->lh]; + if (!bsd->PositionSet) + break; + SurvivePose *lhp = &bsd->Pose; FLT angle = ld->angle; int sensor_id = ld->sensor_id; int axis = dd->sweepaxis; - const SurvivePose * object_pose = &so->OutPose; + const SurvivePose *object_pose = &so->OutPose; dd->sweeplh = lhid; - //FOR NOW, drop LH1. - //if( lhid == 1 ) break; + // FOR NOW, drop LH1. + // if( lhid == 1 ) break; + // const FLT * sensor_normal = &so->sensor_normals[senid*3]; + // FLT sensor_normal_worldspace[3]; + // ApplyPoseToPoint(sensor_normal_worldspace, object_pose, sensor_inpos); -// const FLT * sensor_normal = &so->sensor_normals[senid*3]; -// FLT sensor_normal_worldspace[3]; -// ApplyPoseToPoint(sensor_normal_worldspace, object_pose, sensor_inpos); - - const FLT * sensor_inpos = &so->sensor_locations[senid*3]; + const FLT *sensor_inpos = &so->sensor_locations[senid * 3]; FLT sensor_position_worldspace[3]; - //XXX Once I saw this get pretty wild (When in playback) - //I had to invert the values of sensor_inpos. Not sure why. + // XXX Once I saw this get pretty wild (When in playback) + // I had to invert the values of sensor_inpos. Not sure why. ApplyPoseToPoint(sensor_position_worldspace, object_pose, sensor_inpos); - - //printf( "%f %f %f == > %f %f %f\n", sensor_inpos[0], sensor_inpos[1], sensor_inpos[2], sensor_position_worldspace[0], sensor_position_worldspace[1], sensor_position_worldspace[2] ); + // printf( "%f %f %f == > %f %f %f\n", sensor_inpos[0], sensor_inpos[1], sensor_inpos[2], + // sensor_position_worldspace[0], sensor_position_worldspace[1], sensor_position_worldspace[2] ); // = sensor position, relative to lighthouse center. - FLT sensorpos_rel_lh[3]; - sub3d( sensorpos_rel_lh, sensor_position_worldspace, lhp->Pos ); + FLT sensorpos_rel_lh[3]; + sub3d(sensorpos_rel_lh, sensor_position_worldspace, lhp->Pos); - //Next, define a normal in global space of the plane created by the sweep hit. - //Careful that this must be normalized. + // Next, define a normal in global space of the plane created by the sweep hit. + // Careful that this must be normalized. FLT sweep_normal[3]; - //If 1, the "y" axis. //XXX Check me. - if( axis ) //XXX Just FYI this should include account for skew + // If 1, the "y" axis. //XXX Check me. + if (axis) // XXX Just FYI this should include account for skew { sweep_normal[0] = 0; - sweep_normal[1] = cos(angle ); - sweep_normal[2] = sin( angle ); - //printf( "+" ); - } - else - { - sweep_normal[0] = cos( angle ); + sweep_normal[1] = cos(angle); + sweep_normal[2] = sin(angle); + // printf( "+" ); + } else { + sweep_normal[0] = cos(angle); sweep_normal[1] = 0; - sweep_normal[2] = -sin( angle ); - //printf( "-" ); + sweep_normal[2] = -sin(angle); + // printf( "-" ); } - //Need to apply the lighthouse's transformation to the sweep's normal. - quatrotatevector( sweep_normal, lhp->Rot, sweep_normal); + // Need to apply the lighthouse's transformation to the sweep's normal. + quatrotatevector(sweep_normal, lhp->Rot, sweep_normal); - //Compute point-line distance between sensorpos_rel_lh and the plane defined by sweep_normal. - //Do this by projecting sensorpos_rel_lh (w) onto sweep_normal (v). - //You can do this by |v dot w| / |v| ... But we know |v| is 1. So... - FLT dist = dot3d( sensorpos_rel_lh, sweep_normal ); + // Compute point-line distance between sensorpos_rel_lh and the plane defined by sweep_normal. + // Do this by projecting sensorpos_rel_lh (w) onto sweep_normal (v). + // You can do this by |v dot w| / |v| ... But we know |v| is 1. So... + FLT dist = dot3d(sensorpos_rel_lh, sweep_normal); - if( (i = dd->ptsweep) < MAX_PT_PER_SWEEP ) - { - memcpy( dd->normal_at_errors[i], sweep_normal, sizeof(FLT)*3 ); + if ((i = dd->ptsweep) < MAX_PT_PER_SWEEP) { + memcpy(dd->normal_at_errors[i], sweep_normal, sizeof(FLT) * 3); dd->quantity_errors[i] = dist; dd->angles_at_pts[i] = angle; dd->sensor_ids[i] = sensor_id; - memcpy( &dd->object_pose_at_hit[i], object_pose, sizeof(SurvivePose) ); + memcpy(&dd->object_pose_at_hit[i], object_pose, sizeof(SurvivePose)); dd->ptsweep++; } @@ -133,197 +126,201 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { PoserDataLight *l = (PoserDataLight *)pd; int lhid = l->lh; - - //you can get sweepaxis and sweeplh. - if( dd->ptsweep ) - { + // you can get sweepaxis and sweeplh. + if (dd->ptsweep) { int i; int lhid = dd->sweeplh; int axis = dd->sweepaxis; int pts = dd->ptsweep; - const SurvivePose * object_pose = &so->OutPose; //XXX TODO Should pull pose from approximate time when LHs were scanning it. + const SurvivePose *object_pose = + &so->OutPose; // XXX TODO Should pull pose from approximate time when LHs were scanning it. - BaseStationData * bsd = &so->ctx->bsd[lhid]; - SurvivePose * lh_pose = &bsd->Pose; + BaseStationData *bsd = &so->ctx->bsd[lhid]; + SurvivePose *lh_pose = &bsd->Pose; int validpoints = 0; int ptvalid[MAX_PT_PER_SWEEP]; FLT avgerr = 0.0; - FLT vec_correct[3] = { 0., 0. , 0. }; + FLT vec_correct[3] = {0., 0., 0.}; FLT avgang = 0.0; -//Tunable parameters: -#define MIN_HIT_QUALITY 0.5 //Determines which hits to cull. -#define HIT_QUALITY_BASELINE 0.0001 //Determines which hits to cull. Actually SQRT(baseline) if 0.0001, it is really 1cm +// Tunable parameters: +#define MIN_HIT_QUALITY 0.5 // Determines which hits to cull. +#define HIT_QUALITY_BASELINE \ + 0.0001 // Determines which hits to cull. Actually SQRT(baseline) if 0.0001, it is really 1cm -#define CORRECT_LATERAL_POSITION_COEFFICIENT 0.8 //Explodes if you exceed 1.0 -#define CORRECT_TELESCOPTION_COEFFICIENT 8.0 //Converges even as high as 10.0 and doesn't explode. -#define CORRECT_ROTATION_COEFFICIENT 1.0 //This starts to fall apart above 5.0, but for good reason. It is amplified by the number of points seen. +#define CORRECT_LATERAL_POSITION_COEFFICIENT 0.8 // Explodes if you exceed 1.0 +#define CORRECT_TELESCOPTION_COEFFICIENT 8.0 // Converges even as high as 10.0 and doesn't explode. +#define CORRECT_ROTATION_COEFFICIENT \ + 1.0 // This starts to fall apart above 5.0, but for good reason. It is amplified by the number of points seen. #define ROTATIONAL_CORRECTION_MAXFORCE 0.10 - //Step 1: Determine standard of deviation, and average in order to + // Step 1: Determine standard of deviation, and average in order to // drop points that are likely in error. { - //Calculate average + // Calculate average FLT avgerr_orig = 0.0; FLT stddevsq = 0.0; - for( i = 0; i < pts; i++ ) + for (i = 0; i < pts; i++) avgerr_orig += dd->quantity_errors[i]; - avgerr_orig/=pts; + avgerr_orig /= pts; - //Calculate standard of deviation. - for( i = 0; i < pts; i++ ) - { - FLT diff = dd->quantity_errors[i]-avgerr_orig; - stddevsq += diff*diff; + // Calculate standard of deviation. + for (i = 0; i < pts; i++) { + FLT diff = dd->quantity_errors[i] - avgerr_orig; + stddevsq += diff * diff; } - stddevsq/=pts; + stddevsq /= pts; - for( i = 0; i < pts; i++ ) - { + for (i = 0; i < pts; i++) { FLT err = dd->quantity_errors[i]; - FLT diff = err-avgerr_orig; + FLT diff = err - avgerr_orig; diff *= diff; - int isptvalid = (diff * MIN_HIT_QUALITY <= stddevsq + HIT_QUALITY_BASELINE)?1:0; + int isptvalid = (diff * MIN_HIT_QUALITY <= stddevsq + HIT_QUALITY_BASELINE) ? 1 : 0; ptvalid[i] = isptvalid; - if( isptvalid ) - { + if (isptvalid) { avgang += dd->angles_at_pts[i]; avgerr += err; - validpoints ++; + validpoints++; } } avgang /= validpoints; avgerr /= validpoints; } - //Step 2: Determine average lateral error. - //We can actually always perform this operation. Even with only one point. + // Step 2: Determine average lateral error. + // We can actually always perform this operation. Even with only one point. { - FLT avg_err[3] = { 0, 0, 0 }; //Positional error. - for( i = 0; i < pts; i++ ) - { - if( !ptvalid[i] ) continue; - FLT * nrm = dd->normal_at_errors[i]; + FLT avg_err[3] = {0, 0, 0}; // Positional error. + for (i = 0; i < pts; i++) { + if (!ptvalid[i]) + continue; + FLT *nrm = dd->normal_at_errors[i]; FLT err = dd->quantity_errors[i]; avg_err[0] = avg_err[0] + nrm[0] * err; avg_err[1] = avg_err[1] + nrm[1] * err; avg_err[2] = avg_err[2] + nrm[2] * err; } - //NOTE: The "avg_err" is not geometrically centered. This is actually - //probably okay, since if you have sevearl data points to one side, you - //can probably trust that more. - scale3d(avg_err, avg_err, 1./validpoints); + // NOTE: The "avg_err" is not geometrically centered. This is actually + // probably okay, since if you have sevearl data points to one side, you + // can probably trust that more. + scale3d(avg_err, avg_err, 1. / validpoints); - //We have "Average error" now. A vector in worldspace. - //This can correct for lateral error, but not distance from camera. + // We have "Average error" now. A vector in worldspace. + // This can correct for lateral error, but not distance from camera. - //XXX TODO: Should we check to see if we only have one or - //two points to make sure the error on this isn't unusually high? - //If calculated error is unexpectedly high, then we should probably - //Not apply the transform. - scale3d( avg_err, avg_err, -CORRECT_LATERAL_POSITION_COEFFICIENT ); - add3d( vec_correct, vec_correct, avg_err ); + // XXX TODO: Should we check to see if we only have one or + // two points to make sure the error on this isn't unusually high? + // If calculated error is unexpectedly high, then we should probably + // Not apply the transform. + scale3d(avg_err, avg_err, -CORRECT_LATERAL_POSITION_COEFFICIENT); + add3d(vec_correct, vec_correct, avg_err); } - //Step 3: Control telecoption from lighthouse. + // Step 3: Control telecoption from lighthouse. // we need to find out what the weighting is to determine "zoom" - if( validpoints > 1 ) //Can't correct "zoom" with only one point. + if (validpoints > 1) // Can't correct "zoom" with only one point. { FLT zoom = 0.0; FLT rmsang = 0.0; - for( i = 0; i < pts; i++ ) - { - if( !ptvalid[i] ) continue; + for (i = 0; i < pts; i++) { + if (!ptvalid[i]) + continue; FLT delang = dd->angles_at_pts[i] - avgang; FLT delerr = dd->quantity_errors[i] - avgerr; - if( axis ) delang *= -1; //Flip sign on alternate axis because it's measured backwards. + if (axis) + delang *= -1; // Flip sign on alternate axis because it's measured backwards. zoom += delerr * delang; rmsang += delang * delang; } - //Control into or outof lighthouse. - //XXX Check to see if we need to sqrt( the rmsang), need to check convergance behavior close to lighthouse. - //This is a questionable step. - zoom /= sqrt(rmsang); + // Control into or outof lighthouse. + // XXX Check to see if we need to sqrt( the rmsang), need to check convergance behavior close to + // lighthouse. + // This is a questionable step. + zoom /= sqrt(rmsang); zoom *= CORRECT_TELESCOPTION_COEFFICIENT; FLT veccamalong[3]; - sub3d( veccamalong, lh_pose->Pos, object_pose->Pos ); - normalize3d( veccamalong, veccamalong ); - scale3d( veccamalong, veccamalong, zoom ); - add3d( vec_correct, veccamalong, vec_correct ); + sub3d(veccamalong, lh_pose->Pos, object_pose->Pos); + normalize3d(veccamalong, veccamalong); + scale3d(veccamalong, veccamalong, zoom); + add3d(vec_correct, veccamalong, vec_correct); } - SurvivePose object_pose_out; add3d(object_pose_out.Pos, vec_correct, object_pose->Pos); - quatcopy( object_pose_out.Rot, object_pose->Rot ); + quatcopy(object_pose_out.Rot, object_pose->Rot); - //Stage 4: "Tug" on the rotation of the object, from all of the sensor's pov. - //If we were able to determine likliehood of a hit in the sweep instead of afterward - //we would actually be able to perform this on a per-hit basis. - if( 1 ) { + // Stage 4: "Tug" on the rotation of the object, from all of the sensor's pov. + // If we were able to determine likliehood of a hit in the sweep instead of afterward + // we would actually be able to perform this on a per-hit basis. + if (1) { LinmathQuat correction; - quatcopy( correction, LinmathQuat_Identity ); - for( i = 0; i < pts; i++ ) - { - if( !ptvalid[i] ) continue; - FLT dist = dd->quantity_errors[i]-avgerr; + quatcopy(correction, LinmathQuat_Identity); + for (i = 0; i < pts; i++) { + if (!ptvalid[i]) + continue; + FLT dist = dd->quantity_errors[i] - avgerr; FLT angle = dd->angles_at_pts[i]; int sensor_id = dd->sensor_ids[i]; - FLT * normal = dd->normal_at_errors[i]; - const SurvivePose * object_pose_at_hit = &dd->object_pose_at_hit[i]; - const FLT * sensor_inpos = &so->sensor_locations[sensor_id*3]; + FLT *normal = dd->normal_at_errors[i]; + const SurvivePose *object_pose_at_hit = &dd->object_pose_at_hit[i]; + const FLT *sensor_inpos = &so->sensor_locations[sensor_id * 3]; LinmathQuat world_to_object_space; quatgetreciprocal(world_to_object_space, object_pose_at_hit->Rot); - FLT correction_in_object_space[3]; //The amount across the surface of the object the rotation should happen. + FLT correction_in_object_space[3]; // The amount across the surface of the object the rotation + // should happen. - quatrotatevector(correction_in_object_space, world_to_object_space, normal ); + quatrotatevector(correction_in_object_space, world_to_object_space, normal); dist *= CORRECT_ROTATION_COEFFICIENT; - if( dist > ROTATIONAL_CORRECTION_MAXFORCE ) dist = ROTATIONAL_CORRECTION_MAXFORCE; - if( dist <-ROTATIONAL_CORRECTION_MAXFORCE ) dist =-ROTATIONAL_CORRECTION_MAXFORCE; + if (dist > ROTATIONAL_CORRECTION_MAXFORCE) + dist = ROTATIONAL_CORRECTION_MAXFORCE; + if (dist < -ROTATIONAL_CORRECTION_MAXFORCE) + dist = -ROTATIONAL_CORRECTION_MAXFORCE; - //Now, we have a "tug" vector in object-local space. Need to apply the torque. + // Now, we have a "tug" vector in object-local space. Need to apply the torque. FLT vector_from_center_of_object[3]; - normalize3d( vector_from_center_of_object, sensor_inpos ); - //scale3d(vector_from_center_of_object, sensor_inpos, 10.0 ); - // vector_from_center_of_object[2]*=-1; - // vector_from_center_of_object[1]*=-1; -// vector_from_center_of_object[0]*=-1; - //vector_from_center_of_object - scale3d(vector_from_center_of_object,vector_from_center_of_object, 1); - + normalize3d(vector_from_center_of_object, sensor_inpos); + // scale3d(vector_from_center_of_object, sensor_inpos, 10.0 ); + // vector_from_center_of_object[2]*=-1; + // vector_from_center_of_object[1]*=-1; + // vector_from_center_of_object[0]*=-1; + // vector_from_center_of_object + scale3d(vector_from_center_of_object, vector_from_center_of_object, 1); FLT new_vector_in_object_space[3]; - //printf( "%f %f %f %f\n", object_pose_at_hit->Rot[0], object_pose_at_hit->Rot[1], object_pose_at_hit->Rot[2], object_pose_at_hit->Rot[3] ); - //printf( "%f %f %f // %f %f %f // %f\n", vector_from_center_of_object[0], vector_from_center_of_object[1], vector_from_center_of_object[2], correction_in_object_space[0], correction_in_object_space[1], correction_in_object_space[2], dist ); - scale3d( correction_in_object_space, correction_in_object_space, -dist ); - add3d( new_vector_in_object_space, vector_from_center_of_object, correction_in_object_space ); + // printf( "%f %f %f %f\n", object_pose_at_hit->Rot[0], object_pose_at_hit->Rot[1], + // object_pose_at_hit->Rot[2], object_pose_at_hit->Rot[3] ); + // printf( "%f %f %f // %f %f %f // %f\n", vector_from_center_of_object[0], + // vector_from_center_of_object[1], vector_from_center_of_object[2], correction_in_object_space[0], + // correction_in_object_space[1], correction_in_object_space[2], dist ); + scale3d(correction_in_object_space, correction_in_object_space, -dist); + add3d(new_vector_in_object_space, vector_from_center_of_object, correction_in_object_space); - normalize3d( new_vector_in_object_space, new_vector_in_object_space ); + normalize3d(new_vector_in_object_space, new_vector_in_object_space); LinmathQuat corrective_quaternion; - quatfrom2vectors(corrective_quaternion, vector_from_center_of_object, new_vector_in_object_space ); - quatrotateabout( correction, correction, corrective_quaternion ); - //printf( "%f -> %f %f %f => %f %f %f [%f %f %f %f]\n", dist, vector_from_center_of_object[0], vector_from_center_of_object[1], vector_from_center_of_object[2], - //correction_in_object_space[0], correction_in_object_space[1], correction_in_object_space[2], - //corrective_quaternion[0],corrective_quaternion[1],corrective_quaternion[1],corrective_quaternion[3]); + quatfrom2vectors(corrective_quaternion, vector_from_center_of_object, new_vector_in_object_space); + quatrotateabout(correction, correction, corrective_quaternion); + // printf( "%f -> %f %f %f => %f %f %f [%f %f %f %f]\n", dist, vector_from_center_of_object[0], + // vector_from_center_of_object[1], vector_from_center_of_object[2], + // correction_in_object_space[0], correction_in_object_space[1], correction_in_object_space[2], + // corrective_quaternion[0],corrective_quaternion[1],corrective_quaternion[1],corrective_quaternion[3]); } - //printf( "Applying: %f %f %f %f\n", correction[0], correction[1], correction[2], correction[3] ); - //Apply our corrective quaternion to the output. - quatrotateabout( object_pose_out.Rot, object_pose_out.Rot, correction ); - quatnormalize( object_pose_out.Rot, object_pose_out.Rot ); + // printf( "Applying: %f %f %f %f\n", correction[0], correction[1], correction[2], correction[3] ); + // Apply our corrective quaternion to the output. + quatrotateabout(object_pose_out.Rot, object_pose_out.Rot, correction); + quatnormalize(object_pose_out.Rot, object_pose_out.Rot); } - //Janky need to do this somewhere else... This initializes the pose estimator. - if( so->PoseConfidence < .01 ) - { - memcpy( &object_pose_out, &LinmathPose_Identity, sizeof( LinmathPose_Identity ) ); + // Janky need to do this somewhere else... This initializes the pose estimator. + if (so->PoseConfidence < .01) { + memcpy(&object_pose_out, &LinmathPose_Identity, sizeof(LinmathPose_Identity)); so->PoseConfidence = 1.0; } @@ -332,11 +329,11 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { } dd->sweepaxis = l->acode & 1; - //printf( "SYNC %d %p\n", l->acode, dd ); + // printf( "SYNC %d %p\n", l->acode, dd ); break; } case POSERDATA_FULL_SCENE: { - //return opencv_solver_fullscene(so, (PoserDataFullScene *)(pd)); + // return opencv_solver_fullscene(so, (PoserDataFullScene *)(pd)); } } return -1; diff --git a/src/poser_charlesslow.c b/src/poser_charlesslow.c index f725334..5cac973 100644 --- a/src/poser_charlesslow.c +++ b/src/poser_charlesslow.c @@ -256,7 +256,7 @@ static FLT RunOpti( SurviveObject * hmd, PoserDataFullScene * fs, int lh, int pr int dataindex = p*(2*NUM_LIGHTHOUSES)+lh*2; if( fs->lengths[p][lh][0] < 0 || fs->lengths[p][lh][1] < 0 ) continue; - FLT out[2] = { 0 }; + FLT out[2] = {0}; survive_apply_bsd_calibration(hmd->ctx, lh, fs->angles[p][lh], out); //Find out where our ray shoots forth from. @@ -343,7 +343,7 @@ static FLT RunOpti( SurviveObject * hmd, PoserDataFullScene * fs, int lh, int pr if( fs->lengths[p][lh][0] < 0 || fs->lengths[p][lh][1] < 0 ) continue; //Find out where our ray shoots forth from. - FLT out[2] = { 0 }; + FLT out[2] = {0}; survive_apply_bsd_calibration(hmd->ctx, lh, fs->angles[p][lh], out); // Find out where our ray shoots forth from. diff --git a/src/poser_imu.c b/src/poser_imu.c new file mode 100644 index 0000000..7615170 --- /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_pose_func(pd, so, &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 e74bb20..f0d5645 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; @@ -268,12 +272,12 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive info); // info if (currentPositionValid) { - FLT distp[3]; - sub3d(distp, so->OutPose.Pos, soLocation.Pos); - FLT distance = magnitude3d(distp); - ; - if (distance > 1.) - status = -1; + // FLT distp[3]; + // sub3d(distp, so->OutPose.Pos, soLocation.Pos); + // FLT distance = magnitude3d(distp); + + // if (distance > 1.) + // status = -1; } if (status > 0 && (info[1] / meas_size * 2) < d->max_error) { d->failures_to_reset_cntr = d->failures_to_reset; @@ -292,7 +296,7 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive } } - return info[1] / meas_size * 2; + return status; // info[1] / meas_size * 2; } // Optimizes for LH position assuming object is posed at 0 @@ -392,12 +396,12 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { d->failures_to_reset = survive_configi(ctx, "sba-failures-to-reset", SC_GET, 1); d->successes_to_reset_cntr = 0; d->successes_to_reset = survive_configi(ctx, "sba-successes-to-reset", SC_GET, 100); - + d->useIMU = survive_configi(ctx, "sba-use-imu", SC_GET, 1); d->required_meas = survive_configi(ctx, "sba-required-meas", SC_GET, 8); d->max_error = survive_configf(ctx, "sba-max-error", SC_GET, .0001); d->sensor_time_window = survive_configi(ctx, "sba-time-window", SC_GET, SurviveSensorActivations_default_tolerance * 2); - d->sensor_variance_per_second = survive_configf(ctx, "sba-sensor-variance-per-sec", SC_GET, 0.001); + d->sensor_variance_per_second = survive_configf(ctx, "sba-sensor-variance-per-sec", SC_GET, 10.0); d->sensor_variance = survive_configf(ctx, "sba-sensor-variance", SC_GET, 1.0); d->so = so; @@ -407,6 +411,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { SV_INFO("\tsba-sensor-variance-per-sec: %f", d->sensor_variance_per_second); SV_INFO("\tsba-time-window: %d", d->sensor_time_window); SV_INFO("\tsba-max-error: %f", d->max_error); + SV_INFO("\tsba-use-imu: %d", d->useIMU); } SBAData *d = so->PoserData; switch (pd->pt) { @@ -421,6 +426,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 +435,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 +451,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_pose_func(pd, so, &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/poser_turveytori.c b/src/poser_turveytori.c index 243051e..b2eed7f 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1631,8 +1631,7 @@ static void QuickPose(SurviveObject *so, PoserData *pd, SurvivePose *additionalT SolveForLighthouse(&pose.Pos[0], &pose.Rot[0], to, so, pd, 0, additionalTx, 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->poseproc) - { + if (so->ctx->poseproc) { so->ctx->poseproc(so, lh, &pose); } diff --git a/src/survive.c b/src/survive.c index f6f98fc..b024bbb 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); } @@ -559,11 +558,8 @@ int survive_simple_inflate(struct SurviveContext *ctx, const char *input, int in return len; } - - #endif - const char *survive_object_codename(SurviveObject *so) { return so->codename; } const char *survive_object_drivername(SurviveObject *so) { return so->drivername; } 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..e49da3e --- /dev/null +++ b/src/survive_imu.c @@ -0,0 +1,220 @@ +#include "survive_imu.h" +#include "linmath.h" +#include "survive_internal.h" +#include <survive_imu.h> + +//--------------------------------------------------------------------------------------------------- +// Definitions + +#define sampleFreq 240.0f // sample frequency in Hz +#define twoKpDef (2.0f * 0.5f) // 2 * proportional gain +#define twoKiDef (2.0f * 0.0f) // 2 * integral gain + +//--------------------------------------------------------------------------------------------------- +// Function declarations + +float invSqrt(float x) { + float halfx = 0.5f * x; + float y = x; + long i = *(long *)&y; + i = 0x5f3759df - (i >> 1); + y = *(float *)&i; + y = y * (1.5f - (halfx * y * y)); + return y; +} +//--------------------------------------------------------------------------------------------------- +// IMU algorithm update +// From http://x-io.co.uk/open-source-imu-and-ahrs-algorithms/ +static void MahonyAHRSupdateIMU(SurviveIMUTracker *tracker, float gx, float gy, float gz, float ax, float ay, + float az) { + float recipNorm; + float halfvx, halfvy, halfvz; + float halfex, halfey, halfez; + float qa, qb, qc; + + const float twoKp = twoKpDef; // 2 * proportional gain (Kp) + const float twoKi = twoKiDef; // 2 * integral gain (Ki) + + float q0 = tracker->pose.Rot[0]; + float q1 = tracker->pose.Rot[1]; + float q2 = tracker->pose.Rot[2]; + float q3 = tracker->pose.Rot[3]; + + // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) + if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { + + // Normalise accelerometer measurement + recipNorm = invSqrt(ax * ax + ay * ay + az * az); + ax *= recipNorm; + ay *= recipNorm; + az *= recipNorm; + + // Estimated direction of gravity and vector perpendicular to magnetic flux + halfvx = q1 * q3 - q0 * q2; + halfvy = q0 * q1 + q2 * q3; + halfvz = q0 * q0 - 0.5f + q3 * q3; + + // Error is sum of cross product between estimated and measured direction of gravity + halfex = (ay * halfvz - az * halfvy); + halfey = (az * halfvx - ax * halfvz); + halfez = (ax * halfvy - ay * halfvx); + + // Compute and apply integral feedback if enabled + if (twoKi > 0.0f) { + tracker->integralFBx += twoKi * halfex * (1.0f / sampleFreq); // tracker->integral error scaled by Ki + tracker->integralFBy += twoKi * halfey * (1.0f / sampleFreq); + tracker->integralFBz += twoKi * halfez * (1.0f / sampleFreq); + gx += tracker->integralFBx; // apply tracker->integral feedback + gy += tracker->integralFBy; + gz += tracker->integralFBz; + } else { + tracker->integralFBx = 0.0f; // prevent tracker->integral windup + tracker->integralFBy = 0.0f; + tracker->integralFBz = 0.0f; + } + + // Apply proportional feedback + gx += twoKp * halfex; + gy += twoKp * halfey; + gz += twoKp * halfez; + } + + // Integrate rate of change of quaternion + gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors + gy *= (0.5f * (1.0f / sampleFreq)); + gz *= (0.5f * (1.0f / sampleFreq)); + qa = q0; + qb = q1; + qc = q2; + q0 += (-qb * gx - qc * gy - q3 * gz); + q1 += (qa * gx + qc * gz - q3 * gy); + q2 += (qa * gy - qb * gz + q3 * gx); + q3 += (qa * gz + qb * gy - qc * gx); + + // Normalise quaternion + recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + q0 *= recipNorm; + q1 *= recipNorm; + q2 *= recipNorm; + q3 *= recipNorm; + + tracker->pose.Rot[0] = q0; + tracker->pose.Rot[1] = q1; + tracker->pose.Rot[2] = q2; + tracker->pose.Rot[3] = q3; +} + +static inline uint32_t tick_difference(uint32_t most_recent, uint32_t least_recent) { + uint32_t diff = 0; + if (most_recent > least_recent) { + diff = most_recent - least_recent; + } else { + diff = least_recent - most_recent; + } + + if (diff > 0xFFFFFFFF / 2) + return 0x7FFFFFFF / 2 - diff; + return diff; +} + +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] = 0; + } + //(pose->Pos[i] - tracker->lastGT.Pos[i]) / tick_difference(timecode, tracker->lastGTTime) * 48000000.; + + tracker->integralFBx = tracker->integralFBy = tracker->integralFBz = 0.0; + 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.8066); + FLT m = magnitude3d(rAcc); +} +static void iterate_position(SurviveIMUTracker *tracker, double time_diff, const PoserDataIMU *pIMU, FLT *out) { + const SurvivePose *pose = &tracker->pose; + const FLT *vel = tracker->current_velocity; + + for (int i = 0; i < 3; i++) + out[i] = pose->Pos[i]; + + FLT acc_mul = time_diff * time_diff / 2; + + LinmathVec3d acc; + scale3d(acc, pIMU->accel, tracker->accel_scale_bias); + LinmathVec3d rAcc = {0}; + RotateAccel(rAcc, pose, acc); + scale3d(rAcc, rAcc, acc_mul); + + for (int i = 0; i < 3; i++) { + out[i] += time_diff * vel[i] + rAcc[i]; + } +} + +static void iterate_velocity(LinmathVec3d result, SurviveIMUTracker *tracker, double time_diff, PoserDataIMU *pIMU) { + const SurvivePose *pose = &tracker->pose; + const FLT *vel = tracker->current_velocity; + scale3d(result, vel, 1.); + + LinmathVec3d acc; + scale3d(acc, pIMU->accel, tracker->accel_scale_bias); + + LinmathVec3d rAcc = {0}; + RotateAccel(rAcc, pose, acc); + 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); + tracker->accel_scale_bias = 1. / magnitude3d(tracker->updir); + 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; + } + + float gx = data->gyro[0], gy = data->gyro[1], gz = data->gyro[2]; + float ax = data->accel[0], ay = data->accel[1], az = data->accel[2]; + + MahonyAHRSupdateIMU(tracker, gx, gy, gz, ax, ay, az); + + FLT time_diff = tick_difference(data->timecode, tracker->last_data.timecode) / (FLT)so->timebase_hz; + + if (tick_difference(data->timecode, tracker->lastGTTime) < 3200000 * 3) { + FLT next[3]; + iterate_position(tracker, time_diff, data, next); + + LinmathVec3d v_next; + iterate_velocity(v_next, tracker, time_diff, data); + + scale3d(tracker->current_velocity, v_next, 1); + scale3d(tracker->pose.Pos, next, 1); + } + + tracker->last_data = *data; +} 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 e013327..abde02d 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..1ffb737 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 ); } diff --git a/tools/viz/survive_viewer.js b/tools/viz/survive_viewer.js index d7f80ad..c8a7b23 100644 --- a/tools/viz/survive_viewer.js +++ b/tools/viz/survive_viewer.js @@ -164,8 +164,8 @@ function create_tracked_object(info) { group.sensors = []; if (info.config && info.config.lighthouse_config) { for (var idx in info.config.lighthouse_config.modelPoints) { - var p = info.config.lighthouse_config.modelPoints[idx]; - var pn = info.config.lighthouse_config.modelNormals[idx]; + var p = info.config.lighthouse_config.modelPoints[idx]; + var pn = info.config.lighthouse_config.modelNormals[idx]; var color = idx / info.config.lighthouse_config.modelPoints * 0xFFFFFF; if (idx === 0) color = 0x00ff00; @@ -173,10 +173,11 @@ function create_tracked_object(info) { var newSensor = new THREE.Mesh(sensorGeometry, sensorMaterial); newSensor.position.set(p[0], p[1], p[2]); - var normalGeom = new THREE.Geometry(); - normalGeom.vertices.push(newSensor.position, - new THREE.Vector3(p[0] + pn[0] * .02, p[1] + pn[1] * .02, p[2] + pn[2] * .02)); - var normal= new THREE.Line(normalGeom, new THREE.LineBasicMaterial({color : idx == 4 ? 0xFF0000 : 0x00FF00})); + var normalGeom = new THREE.Geometry(); + normalGeom.vertices.push(newSensor.position, + new THREE.Vector3(p[0] + pn[0] * .02, p[1] + pn[1] * .02, p[2] + pn[2] * .02)); + var normal = + new THREE.Line(normalGeom, new THREE.LineBasicMaterial({color : idx == 4 ? 0xFF0000 : 0x00FF00})); group.add(normal); group.sensors[idx] = sensorMaterial; group.add(newSensor); @@ -299,7 +300,7 @@ var survive_log_handlers = { downAxes[obj.tracker] = new THREE.Geometry(); downAxes[obj.tracker].vertices.push(new THREE.Vector3(0, 0, 0), new THREE.Vector3(0, 0, 0)); - var line = new THREE.Line(downAxes[obj.tracker], new THREE.LineBasicMaterial({color : 0xffffff})); + var line = new THREE.Line(downAxes[obj.tracker], new THREE.LineBasicMaterial({color : 0xffffff})); scene.add(line); } diff --git a/winbuild/getdelim.c b/winbuild/getdelim.c index ca808ab..69020ee 100644 --- a/winbuild/getdelim.c +++ b/winbuild/getdelim.c @@ -30,40 +30,29 @@ THE SOFTWARE. */ - #include <errno.h> #include <limits.h> -#include <stdlib.h> #include <stdio.h> - +#include <stdlib.h> #if __STDC_VERSION__ >= 199901L /* restrict is a keyword */ #else -# define restrict +#define restrict #endif - #ifndef _POSIX_SOURCE typedef long ssize_t; #define SSIZE_MAX LONG_MAX #endif +ssize_t getdelim(char **restrict lineptr, size_t *restrict n, int delimiter, FILE *restrict stream); +ssize_t getline(char **restrict lineptr, size_t *restrict n, FILE *restrict stream); -ssize_t getdelim(char **restrict lineptr, size_t *restrict n, int delimiter, - FILE *restrict stream); -ssize_t getline(char **restrict lineptr, size_t *restrict n, - FILE *restrict stream); - +#define _GETDELIM_GROWBY 128 /* amount to grow line buffer by */ +#define _GETDELIM_MINLEN 4 /* minimum line buffer size */ - -#define _GETDELIM_GROWBY 128 /* amount to grow line buffer by */ -#define _GETDELIM_MINLEN 4 /* minimum line buffer size */ - - -ssize_t getdelim(char **restrict lineptr, size_t *restrict n, int delimiter, - FILE *restrict stream) -{ +ssize_t getdelim(char **restrict lineptr, size_t *restrict n, int delimiter, FILE *restrict stream) { char *buf, *pos; int c; ssize_t bytes; @@ -110,7 +99,7 @@ ssize_t getdelim(char **restrict lineptr, size_t *restrict n, int delimiter, *lineptr = buf; } - *pos++ = (char) c; + *pos++ = (char)c; if (c == delimiter) { break; } @@ -125,19 +114,14 @@ ssize_t getdelim(char **restrict lineptr, size_t *restrict n, int delimiter, return bytes; } - -ssize_t getline(char **restrict lineptr, size_t *restrict n, - FILE *restrict stream) -{ +ssize_t getline(char **restrict lineptr, size_t *restrict n, FILE *restrict stream) { return getdelim(lineptr, n, '\n', stream); } - #ifdef _TEST_GETDELIM /* TODO: this isn't a very extensive test. */ -int main(void) -{ +int main(void) { char *line = NULL; size_t n = 0; while (getline(&line, &n, stdin) > 0) { |