diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/poser_charlesrefine.c | 133 | ||||
-rw-r--r-- | src/poser_epnp.c | 2 | ||||
-rw-r--r-- | src/poser_general_optimizer.c | 2 | ||||
-rw-r--r-- | src/poser_general_optimizer.h | 2 | ||||
-rw-r--r-- | src/poser_mpfit.c | 8 | ||||
-rw-r--r-- | src/poser_sba.c | 11 | ||||
-rw-r--r-- | src/survive.c | 2 | ||||
-rw-r--r-- | src/survive_api.c | 165 | ||||
-rwxr-xr-x | src/survive_cal.c | 1 | ||||
-rw-r--r-- | src/survive_default_devices.c | 54 | ||||
-rw-r--r-- | src/survive_imu.c | 2 | ||||
-rw-r--r-- | src/survive_reproject.c | 2 | ||||
-rw-r--r-- | src/survive_reproject.generated.h | 350 | ||||
-rwxr-xr-x | src/survive_vive.c | 6 |
14 files changed, 449 insertions, 291 deletions
diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index cc188fb..1749d8e 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -25,7 +25,7 @@ typedef struct { uint8_t sensor_ids[MAX_PT_PER_SWEEP]; LinmathPoint3d MixingPositions[NUM_LIGHTHOUSES][2]; - SurvivePose InteralPoseUsedForCalc; //Super high speed vibratey and terrible. + SurvivePose InteralPoseUsedForCalc; // Super high speed vibratey and terrible. float MixingConfidence[NUM_LIGHTHOUSES][2]; int ptsweep; @@ -35,8 +35,7 @@ typedef struct { int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { CharlesPoserData *dd = so->PoserData; - if (!dd) - { + if (!dd) { so->PoserData = dd = calloc(sizeof(CharlesPoserData), 1); SurvivePose object_pose_out; memcpy(&object_pose_out, &LinmathPose_Identity, sizeof(LinmathPose_Identity)); @@ -51,46 +50,43 @@ 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; - imuData->gyro[0] *= 1./240.; - imuData->gyro[1] *= 1./240.; - imuData->gyro[2] *= 1./240.; - quatfromeuler( applymotion, imuData->gyro ); - //printf( "%f %f %f\n", imuData->gyro [0], imuData->gyro [1], imuData->gyro [2] ); + // TODO: Actually do Madgwick's algorithm + LinmathQuat applymotion; + const SurvivePose *object_pose = &so->OutPose; + imuData->gyro[0] *= 1. / 240.; + imuData->gyro[1] *= 1. / 240.; + imuData->gyro[2] *= 1. / 240.; + quatfromeuler(applymotion, imuData->gyro); + // printf( "%f %f %f\n", imuData->gyro [0], imuData->gyro [1], imuData->gyro [2] ); LinmathQuat InvertQuat; quatgetreciprocal(InvertQuat, object_pose->Rot); - //Apply a tiny tug to re-right headset based on the gravity vector. - LinmathVec3d reright = { 0, 0, 1 }; + // Apply a tiny tug to re-right headset based on the gravity vector. + LinmathVec3d reright = {0, 0, 1}; LinmathVec3d normup; - normalize3d( normup, imuData->accel ); + normalize3d(normup, imuData->accel); LinmathVec3d correct_diff; quatrotatevector(reright, InvertQuat, reright); - sub3d( correct_diff, normup, reright ); - scale3d( correct_diff, correct_diff, -0.001 ); //This is the coefficient applying the drag. - add3d( correct_diff, correct_diff, reright ); - LinmathQuat reright_quat; - normalize3d( correct_diff, correct_diff ); - quatfrom2vectors( reright_quat, reright, correct_diff ); - - + sub3d(correct_diff, normup, reright); + scale3d(correct_diff, correct_diff, -0.001); // This is the coefficient applying the drag. + add3d(correct_diff, correct_diff, reright); + LinmathQuat reright_quat; + normalize3d(correct_diff, correct_diff); + quatfrom2vectors(reright_quat, reright, correct_diff); SurvivePose object_pose_out; - quatrotateabout(object_pose_out.Rot, object_pose->Rot, applymotion ); //Contribution from Gyro - quatrotateabout(object_pose_out.Rot, object_pose_out.Rot, reright_quat); //Contribution from Accelerometer + quatrotateabout(object_pose_out.Rot, object_pose->Rot, applymotion); // Contribution from Gyro + quatrotateabout(object_pose_out.Rot, object_pose_out.Rot, reright_quat); // Contribution from Accelerometer quatnormalize(object_pose_out.Rot, object_pose_out.Rot); - - copy3d( object_pose_out.Pos, object_pose->Pos ); + copy3d(object_pose_out.Pos, object_pose->Pos); PoserData_poser_pose_func(pd, so, &object_pose_out); - quatcopy( dd->InteralPoseUsedForCalc.Rot, object_pose_out.Rot); + quatcopy(dd->InteralPoseUsedForCalc.Rot, object_pose_out.Rot); - //PoserDataIMU *imu = (PoserDataIMU *)pd; - //survive_imu_tracker_integrate(so, &dd->tracker, imu); - //PoserData_poser_pose_func(pd, so, &dd->tracker.pose); + // PoserDataIMU *imu = (PoserDataIMU *)pd; + // survive_imu_tracker_integrate(so, &dd->tracker, imu); + // PoserData_poser_pose_func(pd, so, &dd->tracker.pose); return 0; } @@ -106,8 +102,8 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { 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. @@ -177,7 +173,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { int lhid = dd->sweeplh; int axis = dd->sweepaxis; int pts = dd->ptsweep; - //const SurvivePose *object_pose = + // 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]; @@ -195,7 +191,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { 0.0001 // Determines which hits to cull. Actually SQRT(baseline) if 0.0001, it is really 1cm #define CORRECT_LATERAL_POSITION_COEFFICIENT 0.7 // Explodes if you exceed 1.0 -#define CORRECT_TELESCOPTION_COEFFICIENT 7.00 // Converges even as high as 10.0 and doesn't explode. +#define CORRECT_TELESCOPTION_COEFFICIENT 7.00 // Converges even as high as 10.0 and doesn't explode. #define CORRECT_ROTATION_COEFFICIENT \ 0.2 // 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.01 @@ -263,8 +259,6 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { add3d(vec_correct, vec_correct, avg_err); } - - // 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. @@ -297,7 +291,6 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { add3d(vec_correct, veccamalong, vec_correct); } - #if 0 LinmathPoint3d LastDelta; sub3d( LastDelta, dd->MixingPositions[lhid][axis], dd->InteralPoseUsedForCalc.Pos ); @@ -318,9 +311,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { #endif add3d(dd->InteralPoseUsedForCalc.Pos, vec_correct, dd->InteralPoseUsedForCalc.Pos); - - - //quatcopy(object_pose_out.Rot, dd->InteralPoseUsedForCalc.Rot); + // quatcopy(object_pose_out.Rot, dd->InteralPoseUsedForCalc.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 @@ -385,42 +376,46 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { quatnormalize(dd->InteralPoseUsedForCalc.Rot, dd->InteralPoseUsedForCalc.Rot); } - memcpy( dd->MixingPositions[lhid][axis], dd->InteralPoseUsedForCalc.Pos, sizeof( dd->InteralPoseUsedForCalc.Pos ) ); - dd->MixingConfidence[lhid][axis] = (validpoints)?((validpoints>1)?1.0:0.5):0; + memcpy(dd->MixingPositions[lhid][axis], dd->InteralPoseUsedForCalc.Pos, + sizeof(dd->InteralPoseUsedForCalc.Pos)); + dd->MixingConfidence[lhid][axis] = (validpoints) ? ((validpoints > 1) ? 1.0 : 0.5) : 0; - - //Box filter all of the guesstimations, and emit the new guess. + // Box filter all of the guesstimations, and emit the new guess. { FLT MixedAmount = 0; - LinmathPoint3d MixedPosition = { 0, 0, 0 }; + LinmathPoint3d MixedPosition = {0, 0, 0}; int l = 0, a = 0; - if( lhid == 0 && axis == 0 ) for( l = 0; l < NUM_LIGHTHOUSES; l++ ) for( a = 0; a < 2; a++ ) dd->MixingConfidence[l][a] -= 0.1; - - for( l = 0; l < NUM_LIGHTHOUSES; l++ ) for( a = 0; a < 2; a++ ) - { - LinmathPoint3d MixThis = { 0, 0, 0 }; - FLT Confidence = dd->MixingConfidence[l][a]; - if(Confidence < 0 ) Confidence = 0; - scale3d( MixThis, dd->MixingPositions[l][a], Confidence ); - add3d( MixedPosition, MixedPosition, MixThis ); - MixedAmount += Confidence; - //printf( "%f ", Confidence ); - } - scale3d( MixedPosition, MixedPosition, 1./MixedAmount ); - //printf( "%f\n", MixedAmount ); + if (lhid == 0 && axis == 0) + for (l = 0; l < NUM_LIGHTHOUSES; l++) + for (a = 0; a < 2; a++) + dd->MixingConfidence[l][a] -= 0.1; + + for (l = 0; l < NUM_LIGHTHOUSES; l++) + for (a = 0; a < 2; a++) { + LinmathPoint3d MixThis = {0, 0, 0}; + FLT Confidence = dd->MixingConfidence[l][a]; + if (Confidence < 0) + Confidence = 0; + scale3d(MixThis, dd->MixingPositions[l][a], Confidence); + add3d(MixedPosition, MixedPosition, MixThis); + MixedAmount += Confidence; + // printf( "%f ", Confidence ); + } + scale3d(MixedPosition, MixedPosition, 1. / MixedAmount); + // printf( "%f\n", MixedAmount ); SurvivePose object_pose_out; - quatcopy(object_pose_out.Rot, dd->InteralPoseUsedForCalc.Rot ); - copy3d( object_pose_out.Pos, MixedPosition ); + quatcopy(object_pose_out.Rot, dd->InteralPoseUsedForCalc.Rot); + copy3d(object_pose_out.Pos, MixedPosition); PoserData_poser_pose_func(pd, so, &object_pose_out); } - // FLT var_meters = 0.5; - // FLT error = 0.00001; - // FLT var_quat = error + .05; - // FLT var[7] = {error * var_meters, error * var_meters, error * var_meters, error * var_quat, - // error * var_quat, error * var_quat, error * var_quat}; - // - // survive_imu_tracker_integrate_observation(so, l->timecode, &dd->tracker, &object_pose_out, var); - // PoserData_poser_pose_func(pd, so, &dd->tracker.pose); + // FLT var_meters = 0.5; + // FLT error = 0.00001; + // FLT var_quat = error + .05; + // FLT var[7] = {error * var_meters, error * var_meters, error * var_meters, error * var_quat, + // error * var_quat, error * var_quat, error * var_quat}; + // + // survive_imu_tracker_integrate_observation(so, l->timecode, &dd->tracker, &object_pose_out, var); + // PoserData_poser_pose_func(pd, so, &dd->tracker.pose); dd->ptsweep = 0; } diff --git a/src/poser_epnp.c b/src/poser_epnp.c index 851ce29..90fab65 100644 --- a/src/poser_epnp.c +++ b/src/poser_epnp.c @@ -129,7 +129,7 @@ int PoserEPNP(SurviveObject *so, PoserData *pd) { case POSERDATA_LIGHT: { PoserDataLight *lightData = (PoserDataLight *)pd; - SurvivePose posers[2] = { 0 }; + SurvivePose posers[2] = {0}; int meas[2] = {0, 0}; for (int lh = 0; lh < so->ctx->activeLighthouses; lh++) { if (so->ctx->bsd[lh].PositionSet) { diff --git a/src/poser_general_optimizer.c b/src/poser_general_optimizer.c index b3c4e65..ff82fd4 100644 --- a/src/poser_general_optimizer.c +++ b/src/poser_general_optimizer.c @@ -1,8 +1,8 @@ #include "poser_general_optimizer.h" #include "string.h" -#include <malloc.h> #include <assert.h> +#include <malloc.h> #include <stdio.h> void *GetDriver(const char *name); diff --git a/src/poser_general_optimizer.h b/src/poser_general_optimizer.h index 81d94bf..6d4d906 100644 --- a/src/poser_general_optimizer.h +++ b/src/poser_general_optimizer.h @@ -1,5 +1,5 @@ -#include <survive.h> #include <stdlib.h> +#include <survive.h> typedef struct GeneralOptimizerData { int failures_to_reset; diff --git a/src/poser_mpfit.c b/src/poser_mpfit.c index 439d151..45e2c69 100644 --- a/src/poser_mpfit.c +++ b/src/poser_mpfit.c @@ -167,15 +167,15 @@ static double run_mpfit_find_3d_structure(MPFITData *d, PoserDataLight *pdl, Sur } failure_count = 0; - SurvivePose soLocation = { 0 }; + SurvivePose soLocation = {0}; if (!general_optimizer_data_record_current_pose(&d->opt, &pdl->hdr, sizeof(*pdl), &soLocation)) { return -1; } - mp_result result = { 0 }; - mp_par pars[7] = { 0 }; - + mp_result result = {0}; + mp_par pars[7] = {0}; + const bool debug_jacobian = false; if (d->use_jacobian_function) { for (int i = 0; i < 7; i++) { diff --git a/src/poser_sba.c b/src/poser_sba.c index 5166951..8c9e328 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -230,7 +230,7 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive } failure_count = 0; - SurvivePose soLocation = { 0 }; + SurvivePose soLocation = {0}; if (!general_optimizer_data_record_current_pose(&d->opt, &pdl->hdr, sizeof(*pdl), &soLocation)) { return -1; @@ -389,7 +389,6 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { d->sensor_variance = survive_configf(ctx, "sba-sensor-variance", SC_GET, 1.0); d->use_jacobian_function = survive_configi(ctx, "sba-use-jacobian-function", SC_GET, 1.0); - SV_INFO("Initializing SBA:"); SV_INFO("\tsba-required-meas: %d", d->required_meas); SV_INFO("\tsba-sensor-variance: %f", d->sensor_variance); @@ -416,16 +415,14 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { d->last_lh = lightData->lh; d->last_acode = lightData->acode; - if (error < 0) { - } - else { + } else { if (d->useIMU) { FLT var_meters = 0.5; FLT var_quat = error + .05; - FLT var[7] = { error * var_meters, error * var_meters, error * var_meters, error * var_quat, - error * var_quat, error * var_quat, error * var_quat }; + FLT var[7] = {error * var_meters, error * var_meters, error * var_meters, error * var_quat, + error * var_quat, error * var_quat, error * var_quat}; survive_imu_tracker_integrate_observation(so, lightData->timecode, &d->tracker, &estimate, var); estimate = d->tracker.pose; diff --git a/src/survive.c b/src/survive.c index 1ab09da..362adbc 100644 --- a/src/survive.c +++ b/src/survive.c @@ -106,7 +106,7 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) { static int did_manual_driver_registration = 0; if (did_manual_driver_registration == 0) { #define MANUAL_DRIVER_REGISTRATION(func) \ - int func(SurviveObject *so, PoserData *pd); \ + int func(SurviveObject *so, PoserData *pd); \ RegisterDriver(#func, &func); MANUAL_DRIVER_REGISTRATION(PoserCharlesSlow) diff --git a/src/survive_api.c b/src/survive_api.c new file mode 100644 index 0000000..a39524b --- /dev/null +++ b/src/survive_api.c @@ -0,0 +1,165 @@ +#include "survive_api.h" +#include "os_generic.h" +#include "survive.h" +#include "stdio.h" + +struct SurviveSimpleObject { + struct SurviveSimpleContext *actx; + + enum SurviveSimpleObject_type { SurviveSimpleObject_LIGHTHOUSE, SurviveSimpleObject_OBJECT } type; + + union { + int lighthouse; + struct SurviveObject* so; + } data; + + char name[32]; + bool has_update; +}; + +struct SurviveSimpleContext { + SurviveContext* ctx; + + bool running; + og_thread_t thread; + og_mutex_t poll_mutex; + + size_t object_ct; + struct SurviveSimpleObject objects[]; +}; + +static void pose_fn(SurviveObject *so, uint32_t timecode, SurvivePose *pose) { + struct SurviveSimpleContext *actx = so->ctx->user_ptr; + OGLockMutex(actx->poll_mutex); + survive_default_raw_pose_process(so, timecode, pose); + + intptr_t idx = (intptr_t)so->user_ptr; + actx->objects[idx].has_update = true; + OGUnlockMutex(actx->poll_mutex); +} +static void lh_fn(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lighthouse_pose, + SurvivePose *object_pose) { + struct SurviveSimpleContext *actx = ctx->user_ptr; + OGLockMutex(actx->poll_mutex); + survive_default_lighthouse_pose_process(ctx, lighthouse, lighthouse_pose, object_pose); + + actx->objects[lighthouse].has_update = true; + + OGUnlockMutex(actx->poll_mutex); +} + +struct SurviveSimpleContext *survive_simple_init(int argc, char *const *argv) { + SurviveContext* ctx = survive_init(argc, argv); + if (ctx == 0) + return 0; + + survive_startup(ctx); + + int object_ct = ctx->activeLighthouses + ctx->objs_ct; + struct SurviveSimpleContext *actx = + calloc(1, sizeof(struct SurviveSimpleContext) + sizeof(struct SurviveSimpleObject) * object_ct); + actx->object_ct = object_ct; + actx->ctx = ctx; + actx->poll_mutex = OGCreateMutex(); + ctx->user_ptr = actx; + intptr_t i = 0; + for (i = 0; i < ctx->activeLighthouses; i++) { + struct SurviveSimpleObject *obj = &actx->objects[i]; + obj->data.lighthouse = i; + obj->type = SurviveSimpleObject_LIGHTHOUSE; + obj->actx = actx; + obj->has_update = ctx->bsd[i].PositionSet; + snprintf(obj->name, 32, "LH%ld", i); + } + for (; i < object_ct; i++) { + struct SurviveSimpleObject *obj = &actx->objects[i]; + int so_idx = i - ctx->activeLighthouses; + obj->data.so = ctx->objs[so_idx]; + obj->type = SurviveSimpleObject_OBJECT; + obj->actx = actx; + obj->data.so->user_ptr = (void*)i; + snprintf(obj->name, 32, "%s", obj->data.so->codename); + } + + survive_install_pose_fn(ctx, pose_fn); + survive_install_lighthouse_pose_fn(ctx, lh_fn); + return actx; +} + +int survive_simple_stop_thread(struct SurviveSimpleContext *actx) { + actx->running = false; + intptr_t error = (intptr_t)OGJoinThread(actx->thread); + if (error != 0) { + SurviveContext *ctx = actx->ctx; + SV_INFO("Warning: Loope exited with error %ld", error); + } + return error; +} + +void survive_simple_close(struct SurviveSimpleContext *actx) { + if (actx->running) { + survive_simple_stop_thread(actx); + } + + survive_close(actx->ctx); +} + +static inline void *__simple_thread(void *_actx) { + struct SurviveSimpleContext *actx = _actx; + intptr_t error = 0; + while (actx->running && error == 0) { + error = survive_poll(actx->ctx); + } + actx->running = false; + return (void*)error; +} +bool survive_simple_is_running(struct SurviveSimpleContext *actx) { return actx->running; } +void survive_simple_start_thread(struct SurviveSimpleContext *actx) { + actx->running = true; + actx->thread = OGCreateThread(__simple_thread, actx); +} + +const struct SurviveSimpleObject *survive_simple_get_next_object(struct SurviveSimpleContext *actx, + const struct SurviveSimpleObject *curr) { + const struct SurviveSimpleObject *next = curr + 1; + if (next >= actx->objects + actx->object_ct) + return 0; + return next; +} + +const struct SurviveSimpleObject *survive_simple_get_first_object(struct SurviveSimpleContext *actx) { + return actx->objects; +} + +const struct SurviveSimpleObject *survive_simple_get_next_updated(struct SurviveSimpleContext *actx) { + for (int i = 0; i < actx->object_ct; i++) { + if (actx->objects[i].has_update) { + actx->objects[i].has_update = false; + return &actx->objects[i]; + } + } + return 0; +} + +uint32_t survive_simple_object_get_latest_pose(const struct SurviveSimpleObject *sao, SurvivePose *pose) { + uint32_t timecode = 0; + OGLockMutex(sao->actx->poll_mutex); + + switch (sao->type) { + case SurviveSimpleObject_LIGHTHOUSE: { + if(pose) + *pose = sao->actx->ctx->bsd[sao->data.lighthouse].Pose; + break; + } + case SurviveSimpleObject_OBJECT: + if(pose) + *pose = sao->data.so->OutPose; + timecode = sao->data.so->OutPose_timecode; + break; + } + + OGUnlockMutex(sao->actx->poll_mutex); + return timecode; +} + +const char *survive_simple_object_name(const SurviveSimpleObject *sao) { return sao->name; } diff --git a/src/survive_cal.c b/src/survive_cal.c index 4d26bfb..faf2ac7 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -188,7 +188,6 @@ void survive_cal_install( struct SurviveContext * ctx ) } } - const char * DriverName; cd->ConfigPoserFn = GetDriverByConfig(ctx, "Poser", "configposer", "SBA", 0); ootx_packet_clbk = ootx_packet_clbk_d; diff --git a/src/survive_default_devices.c b/src/survive_default_devices.c index 954c47a..4fa3284 100644 --- a/src/survive_default_devices.c +++ b/src/survive_default_devices.c @@ -170,49 +170,49 @@ int survive_load_htc_config_format(SurviveObject *so, char *ct0conf, int len) { } } - - //Handle device-specific sacling. - if( strcmp( so->codename, "HMD" ) == 0 ) - { - if( so->acc_scale ) - { - so->acc_scale[0] *= -1./8192.0; - so->acc_scale[1] *= -1./8192.0; - so->acc_scale[2] *= 1./8192.0; + // Handle device-specific sacling. + if (strcmp(so->codename, "HMD") == 0) { + if (so->acc_scale) { + so->acc_scale[0] *= -1. / 8192.0; + so->acc_scale[1] *= -1. / 8192.0; + so->acc_scale[2] *= 1. / 8192.0; } - if( so->acc_bias ) scale3d( so->acc_bias, so->acc_bias, 2./1000. ); //Odd but seems right. - if( so->gyro_scale ) - { + if (so->acc_bias) + scale3d(so->acc_bias, so->acc_bias, 2. / 1000.); // Odd but seems right. + if (so->gyro_scale) { so->gyro_scale[0] *= -0.000065665; so->gyro_scale[1] *= -0.000065665; - so->gyro_scale[2] *= 0.000065665; + so->gyro_scale[2] *= 0.000065665; } - } - else if( memcmp( so->codename, "WM", 2 ) == 0 ) - { - if( so->acc_scale ) scale3d( so->acc_scale, so->acc_scale, 2./8192.0 ); - if( so->acc_bias ) scale3d( so->acc_bias, so->acc_bias, 2./1000. ); //Need to verify. - if( so->gyro_scale ) scale3d( so->gyro_scale, so->gyro_scale, 3.14159 / 1800. / 1.8 ); //??! 1.8 feels right but why?! + } else if (memcmp(so->codename, "WM", 2) == 0) { + if (so->acc_scale) + scale3d(so->acc_scale, so->acc_scale, 2. / 8192.0); + if (so->acc_bias) + scale3d(so->acc_bias, so->acc_bias, 2. / 1000.); // Need to verify. + if (so->gyro_scale) + scale3d(so->gyro_scale, so->gyro_scale, 3.14159 / 1800. / 1.8); //??! 1.8 feels right but why?! int j; for (j = 0; j < so->sensor_ct; j++) { so->sensor_locations[j * 3 + 0] *= 1.0; } - } - else //Verified on WW, Need to verify on Tracker. + } else // Verified on WW, Need to verify on Tracker. { - //1G for accelerometer, from MPU6500 datasheet - //this can change if the firmware changes the sensitivity. + // 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 - if( so->acc_scale ) scale3d( so->acc_scale, so->acc_scale, 2./8192.0 ); + if (so->acc_scale) + scale3d(so->acc_scale, so->acc_scale, 2. / 8192.0); - //If any other device, we know we at least need this. + // If any other device, we know we at least need this. // I deeply suspect bias is in milligravities -JB - if( so->acc_bias ) scale3d( so->acc_bias, so->acc_bias, 1./1000. ); + if (so->acc_bias) + scale3d(so->acc_bias, so->acc_bias, 1. / 1000.); // From datasheet, can be 250, 500, 1000, 2000 deg/s range over 16 bits // FLT deg_per_sec = 250; - if( so->gyro_scale )scale3d( so->gyro_scale, so->gyro_scale, 3.14159 / 1800. ); + if (so->gyro_scale) + scale3d(so->gyro_scale, so->gyro_scale, 3.14159 / 1800.); } char fname[64]; diff --git a/src/survive_imu.c b/src/survive_imu.c index 9cd0322..3622b2d 100644 --- a/src/survive_imu.c +++ b/src/survive_imu.c @@ -182,7 +182,7 @@ void survive_imu_tracker_integrate_observation(SurviveObject *so, uint32_t timec tracker->pose = *pose; return; } - + // Kalman filter assuming: // F -> Identity // H -> Identity diff --git a/src/survive_reproject.c b/src/survive_reproject.c index 921386b..ae946fe 100644 --- a/src/survive_reproject.c +++ b/src/survive_reproject.c @@ -163,5 +163,5 @@ void survive_apply_bsd_calibration(SurviveContext *ctx, int lh, const FLT *in, F void survive_reproject_from_pose_with_config(const SurviveContext *ctx, struct survive_calibration_config *config, int lighthouse, const SurvivePose *pose, FLT *point3d, FLT *out) { - return survive_reproject_from_pose_with_bsd(&ctx->bsd[lighthouse], config, pose, point3d, out); + survive_reproject_from_pose_with_bsd(&ctx->bsd[lighthouse], config, pose, point3d, out); } diff --git a/src/survive_reproject.generated.h b/src/survive_reproject.generated.h index e184775..6d834f7 100644 --- a/src/survive_reproject.generated.h +++ b/src/survive_reproject.generated.h @@ -1,6 +1,9 @@ - // NOTE: Auto-generated code; see tools/generate_reprojection_functions +// NOTE: Auto-generated code; see tools/generate_reprojection_functions #include <math.h> -static inline void gen_reproject_jac_obj_p(FLT* out, const FLT *obj, const FLT *sensor, const FLT *lh, FLT phase_scale, FLT phase_0, FLT phase_1, FLT tilt_scale, FLT tilt_0, FLT tilt_1, FLT curve_scale, FLT curve_0, FLT curve_1, FLT gib_scale, FLT gibPhase_0, FLT gibPhase_1, FLT gibMag_0, FLT gibMag_1) { +static inline void gen_reproject_jac_obj_p(FLT *out, const FLT *obj, const FLT *sensor, const FLT *lh, FLT phase_scale, + FLT phase_0, FLT phase_1, FLT tilt_scale, FLT tilt_0, FLT tilt_1, + FLT curve_scale, FLT curve_0, FLT curve_1, FLT gib_scale, FLT gibPhase_0, + FLT gibPhase_1, FLT gibMag_0, FLT gibMag_1) { FLT obj_px = *(obj++); FLT obj_py = *(obj++); FLT obj_pz = *(obj++); @@ -18,196 +21,195 @@ static inline void gen_reproject_jac_obj_p(FLT* out, const FLT *obj, const FLT * FLT lh_qi = *(lh++); FLT lh_qj = *(lh++); FLT lh_qk = *(lh++); - FLT x0 = tan(tilt_0*tilt_scale); - FLT x1 = lh_qi*lh_qj; - FLT x2 = lh_qk*lh_qw; + FLT x0 = tan(tilt_0 * tilt_scale); + FLT x1 = lh_qi * lh_qj; + FLT x2 = lh_qk * lh_qw; FLT x3 = x1 - x2; FLT x4 = (lh_qi * lh_qi); FLT x5 = (lh_qj * lh_qj); FLT x6 = x4 + x5; FLT x7 = (lh_qk * lh_qk); FLT x8 = sqrt((lh_qw * lh_qw) + x6 + x7); - FLT x9 = lh_qi*lh_qk; - FLT x10 = lh_qj*lh_qw; + FLT x9 = lh_qi * lh_qk; + FLT x10 = lh_qj * lh_qw; FLT x11 = x10 + x9; - FLT x12 = 2*lh_px*x8; - FLT x13 = lh_qj*lh_qk; - FLT x14 = lh_qi*lh_qw; + FLT x12 = 2 * lh_px * x8; + FLT x13 = lh_qj * lh_qk; + FLT x14 = lh_qi * lh_qw; FLT x15 = x13 - x14; - FLT x16 = 2*lh_py*x8; - FLT x17 = 2*x8; - FLT x18 = x17*x6 - 1; - FLT x19 = obj_qi*obj_qk; - FLT x20 = obj_qj*obj_qw; - FLT x21 = sensor_z*(x19 + x20); + FLT x16 = 2 * lh_py * x8; + FLT x17 = 2 * x8; + FLT x18 = x17 * x6 - 1; + FLT x19 = obj_qi * obj_qk; + FLT x20 = obj_qj * obj_qw; + FLT x21 = sensor_z * (x19 + x20); FLT x22 = (obj_qi * obj_qi); FLT x23 = (obj_qj * obj_qj); FLT x24 = x22 + x23; FLT x25 = (obj_qk * obj_qk); FLT x26 = sqrt((obj_qw * obj_qw) + x24 + x25); - FLT x27 = 2*x26; - FLT x28 = obj_qi*obj_qj; - FLT x29 = obj_qk*obj_qw; - FLT x30 = sensor_y*(x28 - x29); + FLT x27 = 2 * x26; + FLT x28 = obj_qi * obj_qj; + FLT x29 = obj_qk * obj_qw; + FLT x30 = sensor_y * (x28 - x29); FLT x31 = x23 + x25; - FLT x32 = obj_px - sensor_x*(x27*x31 - 1) + x21*x27 + x27*x30; - FLT x33 = 2*x32*x8; - FLT x34 = sensor_x*(x28 + x29); - FLT x35 = obj_qj*obj_qk; - FLT x36 = obj_qi*obj_qw; - FLT x37 = sensor_z*(x35 - x36); + FLT x32 = obj_px - sensor_x * (x27 * x31 - 1) + x21 * x27 + x27 * x30; + FLT x33 = 2 * x32 * x8; + FLT x34 = sensor_x * (x28 + x29); + FLT x35 = obj_qj * obj_qk; + FLT x36 = obj_qi * obj_qw; + FLT x37 = sensor_z * (x35 - x36); FLT x38 = x22 + x25; - FLT x39 = obj_py - sensor_y*(x27*x38 - 1) + x27*x34 + x27*x37; - FLT x40 = 2*x39*x8; - FLT x41 = sensor_y*(x35 + x36); - FLT x42 = sensor_x*(x19 - x20); - FLT x43 = obj_pz - sensor_z*(x24*x27 - 1) + x27*x41 + x27*x42; - FLT x44 = lh_pz*x18 - x11*x12 + x11*x33 - x15*x16 + x15*x40 - x18*x43; - FLT x45 = 1.0/x44; - FLT x46 = 2*x45*x8; + FLT x39 = obj_py - sensor_y * (x27 * x38 - 1) + x27 * x34 + x27 * x37; + FLT x40 = 2 * x39 * x8; + FLT x41 = sensor_y * (x35 + x36); + FLT x42 = sensor_x * (x19 - x20); + FLT x43 = obj_pz - sensor_z * (x24 * x27 - 1) + x27 * x41 + x27 * x42; + FLT x44 = lh_pz * x18 - x11 * x12 + x11 * x33 - x15 * x16 + x15 * x40 - x18 * x43; + FLT x45 = 1.0 / x44; + FLT x46 = 2 * x45 * x8; FLT x47 = x13 + x14; - FLT x48 = 2*lh_pz*x8; - FLT x49 = x17*(x4 + x7) - 1; - FLT x50 = 2*x43*x8; - FLT x51 = lh_py*x49 - x12*x3 + x3*x33 - x39*x49 - x47*x48 + x47*x50; + FLT x48 = 2 * lh_pz * x8; + FLT x49 = x17 * (x4 + x7) - 1; + FLT x50 = 2 * x43 * x8; + FLT x51 = lh_py * x49 - x12 * x3 + x3 * x33 - x39 * x49 - x47 * x48 + x47 * x50; FLT x52 = 1. / (x44 * x44); - FLT x53 = 4*curve_0*curve_scale*x51*x52*x8; - FLT x54 = x11*x52; - FLT x55 = (x51*x51); - FLT x56 = curve_0*x55; - FLT x57 = 1. / (x44*x44*x44); - FLT x58 = 4*curve_scale*x11*x57*x8; + FLT x53 = 4 * curve_0 * curve_scale * x51 * x52 * x8; + FLT x54 = x11 * x52; + FLT x55 = (x51 * x51); + FLT x56 = curve_0 * x55; + FLT x57 = 1. / (x44 * x44 * x44); + FLT x58 = 4 * curve_scale * x11 * x57 * x8; FLT x59 = x1 + x2; FLT x60 = -x10 + x9; - FLT x61 = x17*(x5 + x7) - 1; - FLT x62 = lh_px*x61 - x16*x59 - x32*x61 + x40*x59 - x48*x60 + x50*x60; + FLT x61 = x17 * (x5 + x7) - 1; + FLT x62 = lh_px * x61 - x16 * x59 - x32 * x61 + x40 * x59 - x48 * x60 + x50 * x60; FLT x63 = (x62 * x62); - FLT x64 = 1.0/(x52*x63 + 1); - FLT x65 = x45*x61; - FLT x66 = 2*x54*x62*x8; - FLT x67 = x64*(x65 + x66); - FLT x68 = gibMag_0*gib_scale*cos(gibPhase_0 + atan(x45*x62)); - FLT x69 = x45*x49; - FLT x70 = 2*x15*x8; - FLT x71 = x51*x52; - FLT x72 = x70*x71; - FLT x73 = 4*curve_scale*x15*x57*x8; - FLT x74 = 2*x64; - FLT x75 = x45*x8; - FLT x76 = x74*(-x15*x52*x62*x8 + x59*x75); - FLT x77 = x46*x47; - FLT x78 = x18*x71; - FLT x79 = 2*curve_scale*x18*x57; - FLT x80 = x46*x60; - FLT x81 = x52*x62; - FLT x82 = x18*x81; - FLT x83 = x64*(x80 + x82); - FLT x84 = 2*x0; - FLT x85 = obj_qj*x26; - FLT x86 = sensor_x*x85; - FLT x87 = obj_qi*x26; - FLT x88 = sensor_y*x87; - FLT x89 = sensor_z*x24; - FLT x90 = 1.0/x26; - FLT x91 = obj_qw*x90; - FLT x92 = -x41*x91 - x42*x91 + x86 - x88 + x89*x91; - FLT x93 = 2*x8*x92; - FLT x94 = obj_qk*x26; - FLT x95 = sensor_y*x94; - FLT x96 = sensor_z*x85; - FLT x97 = sensor_x*x31; - FLT x98 = -x21*x91 - x30*x91 + x91*x97 + x95 - x96; - FLT x99 = 2*x8*x98; - FLT x100 = sensor_x*x94; - FLT x101 = sensor_z*x87; - FLT x102 = sensor_y*x38; - FLT x103 = x100 - x101 - x102*x91 + x34*x91 + x37*x91; - FLT x104 = x103*x49 + x3*x99 + x47*x93; - FLT x105 = x104*x45; - FLT x106 = 4*curve_0*curve_scale*x51*x52; - FLT x107 = -x103*x70 + x11*x99 - x18*x92; - FLT x108 = x107*x71; - FLT x109 = 4*curve_scale*x107*x57; - FLT x110 = 2*x59*x8; - FLT x111 = x103*x110 - x60*x93 + x61*x98; - FLT x112 = x74*(x107*x81 + x111*x45); - FLT x113 = sensor_y*x85; - FLT x114 = sensor_z*x94; - FLT x115 = obj_qi*x90; - FLT x116 = -x113 - x114 - x115*x21 - x115*x30 + x115*x97; - FLT x117 = 2*x116*x8; - FLT x118 = obj_qw*x26; - FLT x119 = sensor_y*x118; - FLT x120 = obj_qi*x27; - FLT x121 = -sensor_z*(x115*x24 + x120) + x100 + x115*x41 + x115*x42 + x119; - FLT x122 = 2*x121*x8; - FLT x123 = sensor_z*x118; - FLT x124 = -sensor_y*(x115*x38 + x120) + x115*x34 + x115*x37 - x123 + x86; - FLT x125 = x117*x3 - x122*x47 + x124*x49; - FLT x126 = x125*x45; - FLT x127 = x11*x117 + x121*x18 - x124*x70; - FLT x128 = x127*x71; - FLT x129 = 4*curve_0*curve_scale*x55*x57; - FLT x130 = x110*x124 + x116*x61 + x122*x60; - FLT x131 = x74*(x127*x81 + x130*x45); - FLT x132 = sensor_x*x87; - FLT x133 = obj_qj*x90; - FLT x134 = -x102*x133 + x114 + x132 + x133*x34 + x133*x37; - FLT x135 = obj_qj*x27; - FLT x136 = -sensor_x*(x133*x31 + x135) + x123 + x133*x21 + x133*x30 + x88; - FLT x137 = 2*x136*x8; - FLT x138 = sensor_x*x118; - FLT x139 = -sensor_z*(x133*x24 + x135) + x133*x41 + x133*x42 - x138 + x95; - FLT x140 = 2*x139*x8; - FLT x141 = -x134*x49 + x137*x3 + x140*x47; - FLT x142 = x141*x45; - FLT x143 = x11*x137 + x134*x70 - x139*x18; - FLT x144 = x143*x71; - FLT x145 = x110*x134 - x136*x61 + x140*x60; - FLT x146 = x74*(-x143*x81 + x145*x45); - FLT x147 = obj_qk*x90; - FLT x148 = x113 + x132 + x147*x41 + x147*x42 - x147*x89; - FLT x149 = 2*x148*x8; - FLT x150 = obj_qk*x27; - FLT x151 = -sensor_x*(x147*x31 + x150) + x101 - x119 + x147*x21 + x147*x30; - FLT x152 = 2*x151*x8; - FLT x153 = -sensor_y*(x147*x38 + x150) + x138 + x147*x34 + x147*x37 + x96; - FLT x154 = x149*x47 + x152*x3 - x153*x49; - FLT x155 = x154*x45; - FLT x156 = x11*x152 - x148*x18 + x153*x70; - FLT x157 = x156*x71; - FLT x158 = x110*x153 + x149*x60 - x151*x61; - FLT x159 = x74*(-x156*x81 + x158*x45); - FLT x160 = tan(tilt_1*tilt_scale); - FLT x161 = curve_1*x63; - FLT x162 = 1.0/(x52*x55 + 1); - FLT x163 = 2*x162; - FLT x164 = x163*(x3*x75 - x51*x54*x8); - FLT x165 = gibMag_1*gib_scale*cos(gibPhase_1 - atan(x45*x51)); - FLT x166 = 4*curve_1*curve_scale*x52*x62*x8; - FLT x167 = x162*(x69 + x72); - FLT x168 = x162*(x77 + x78); - FLT x169 = 2*x160*x45; - FLT x170 = 4*curve_1*curve_scale*x52*x62; - FLT x171 = 2*x160*x52*x62; - FLT x172 = x163*(-x105 + x108); - FLT x173 = 4*curve_1*curve_scale*x57*x63; - FLT x174 = x163*(-x126 + x128); - FLT x175 = x163*(x142 - x144); - FLT x176 = x163*(x155 - x157); - *(out++) = x0*x3*x46 - 2*x0*x51*x54*x8 - x3*x53 + x56*x58 + x67*x68 - x67; - *(out++) = 2*curve_0*curve_scale*x49*x51*x52 - x0*x69 - x0*x72 + x56*x73 - x68*x76 + x76; - *(out++) = x0*x77 + x0*x78 - x47*x53 - x56*x79 - x68*x83 + x83; - *(out++) = x104*x106 - x105*x84 + x108*x84 - x109*x56 - x112*x68 + x112; - *(out++) = x106*x125 - x126*x84 - x127*x129 + x128*x84 - x131*x68 + x131; - *(out++) = -x106*x141 + x129*x143 + x142*x84 - x144*x84 - x146*x68 + x146; - *(out++) = -x106*x154 + x129*x156 + x155*x84 - x157*x84 - x159*x68 + x159; - *(out++) = 2*curve_1*curve_scale*x52*x61*x62 + x160*x65 + x160*x66 + x161*x58 + x164*x165 - x164; - *(out++) = -x110*x160*x45 + x160*x52*x62*x70 + x161*x73 - x165*x167 - x166*x59 + x167; - *(out++) = -x160*x80 - x160*x82 - x161*x79 + x165*x168 - x166*x60 - x168; - *(out++) = -x107*x171 - x109*x161 - x111*x169 - x111*x170 + x165*x172 - x172; - *(out++) = -x127*x171 - x127*x173 - x130*x169 - x130*x170 + x165*x174 - x174; - *(out++) = x143*x171 + x143*x173 - x145*x169 - x145*x170 + x165*x175 - x175; - *(out++) = x156*x171 + x156*x173 - x158*x169 - x158*x170 + x165*x176 - x176; + FLT x64 = 1.0 / (x52 * x63 + 1); + FLT x65 = x45 * x61; + FLT x66 = 2 * x54 * x62 * x8; + FLT x67 = x64 * (x65 + x66); + FLT x68 = gibMag_0 * gib_scale * cos(gibPhase_0 + atan(x45 * x62)); + FLT x69 = x45 * x49; + FLT x70 = 2 * x15 * x8; + FLT x71 = x51 * x52; + FLT x72 = x70 * x71; + FLT x73 = 4 * curve_scale * x15 * x57 * x8; + FLT x74 = 2 * x64; + FLT x75 = x45 * x8; + FLT x76 = x74 * (-x15 * x52 * x62 * x8 + x59 * x75); + FLT x77 = x46 * x47; + FLT x78 = x18 * x71; + FLT x79 = 2 * curve_scale * x18 * x57; + FLT x80 = x46 * x60; + FLT x81 = x52 * x62; + FLT x82 = x18 * x81; + FLT x83 = x64 * (x80 + x82); + FLT x84 = 2 * x0; + FLT x85 = obj_qj * x26; + FLT x86 = sensor_x * x85; + FLT x87 = obj_qi * x26; + FLT x88 = sensor_y * x87; + FLT x89 = sensor_z * x24; + FLT x90 = 1.0 / x26; + FLT x91 = obj_qw * x90; + FLT x92 = -x41 * x91 - x42 * x91 + x86 - x88 + x89 * x91; + FLT x93 = 2 * x8 * x92; + FLT x94 = obj_qk * x26; + FLT x95 = sensor_y * x94; + FLT x96 = sensor_z * x85; + FLT x97 = sensor_x * x31; + FLT x98 = -x21 * x91 - x30 * x91 + x91 * x97 + x95 - x96; + FLT x99 = 2 * x8 * x98; + FLT x100 = sensor_x * x94; + FLT x101 = sensor_z * x87; + FLT x102 = sensor_y * x38; + FLT x103 = x100 - x101 - x102 * x91 + x34 * x91 + x37 * x91; + FLT x104 = x103 * x49 + x3 * x99 + x47 * x93; + FLT x105 = x104 * x45; + FLT x106 = 4 * curve_0 * curve_scale * x51 * x52; + FLT x107 = -x103 * x70 + x11 * x99 - x18 * x92; + FLT x108 = x107 * x71; + FLT x109 = 4 * curve_scale * x107 * x57; + FLT x110 = 2 * x59 * x8; + FLT x111 = x103 * x110 - x60 * x93 + x61 * x98; + FLT x112 = x74 * (x107 * x81 + x111 * x45); + FLT x113 = sensor_y * x85; + FLT x114 = sensor_z * x94; + FLT x115 = obj_qi * x90; + FLT x116 = -x113 - x114 - x115 * x21 - x115 * x30 + x115 * x97; + FLT x117 = 2 * x116 * x8; + FLT x118 = obj_qw * x26; + FLT x119 = sensor_y * x118; + FLT x120 = obj_qi * x27; + FLT x121 = -sensor_z * (x115 * x24 + x120) + x100 + x115 * x41 + x115 * x42 + x119; + FLT x122 = 2 * x121 * x8; + FLT x123 = sensor_z * x118; + FLT x124 = -sensor_y * (x115 * x38 + x120) + x115 * x34 + x115 * x37 - x123 + x86; + FLT x125 = x117 * x3 - x122 * x47 + x124 * x49; + FLT x126 = x125 * x45; + FLT x127 = x11 * x117 + x121 * x18 - x124 * x70; + FLT x128 = x127 * x71; + FLT x129 = 4 * curve_0 * curve_scale * x55 * x57; + FLT x130 = x110 * x124 + x116 * x61 + x122 * x60; + FLT x131 = x74 * (x127 * x81 + x130 * x45); + FLT x132 = sensor_x * x87; + FLT x133 = obj_qj * x90; + FLT x134 = -x102 * x133 + x114 + x132 + x133 * x34 + x133 * x37; + FLT x135 = obj_qj * x27; + FLT x136 = -sensor_x * (x133 * x31 + x135) + x123 + x133 * x21 + x133 * x30 + x88; + FLT x137 = 2 * x136 * x8; + FLT x138 = sensor_x * x118; + FLT x139 = -sensor_z * (x133 * x24 + x135) + x133 * x41 + x133 * x42 - x138 + x95; + FLT x140 = 2 * x139 * x8; + FLT x141 = -x134 * x49 + x137 * x3 + x140 * x47; + FLT x142 = x141 * x45; + FLT x143 = x11 * x137 + x134 * x70 - x139 * x18; + FLT x144 = x143 * x71; + FLT x145 = x110 * x134 - x136 * x61 + x140 * x60; + FLT x146 = x74 * (-x143 * x81 + x145 * x45); + FLT x147 = obj_qk * x90; + FLT x148 = x113 + x132 + x147 * x41 + x147 * x42 - x147 * x89; + FLT x149 = 2 * x148 * x8; + FLT x150 = obj_qk * x27; + FLT x151 = -sensor_x * (x147 * x31 + x150) + x101 - x119 + x147 * x21 + x147 * x30; + FLT x152 = 2 * x151 * x8; + FLT x153 = -sensor_y * (x147 * x38 + x150) + x138 + x147 * x34 + x147 * x37 + x96; + FLT x154 = x149 * x47 + x152 * x3 - x153 * x49; + FLT x155 = x154 * x45; + FLT x156 = x11 * x152 - x148 * x18 + x153 * x70; + FLT x157 = x156 * x71; + FLT x158 = x110 * x153 + x149 * x60 - x151 * x61; + FLT x159 = x74 * (-x156 * x81 + x158 * x45); + FLT x160 = tan(tilt_1 * tilt_scale); + FLT x161 = curve_1 * x63; + FLT x162 = 1.0 / (x52 * x55 + 1); + FLT x163 = 2 * x162; + FLT x164 = x163 * (x3 * x75 - x51 * x54 * x8); + FLT x165 = gibMag_1 * gib_scale * cos(gibPhase_1 - atan(x45 * x51)); + FLT x166 = 4 * curve_1 * curve_scale * x52 * x62 * x8; + FLT x167 = x162 * (x69 + x72); + FLT x168 = x162 * (x77 + x78); + FLT x169 = 2 * x160 * x45; + FLT x170 = 4 * curve_1 * curve_scale * x52 * x62; + FLT x171 = 2 * x160 * x52 * x62; + FLT x172 = x163 * (-x105 + x108); + FLT x173 = 4 * curve_1 * curve_scale * x57 * x63; + FLT x174 = x163 * (-x126 + x128); + FLT x175 = x163 * (x142 - x144); + FLT x176 = x163 * (x155 - x157); + *(out++) = x0 * x3 * x46 - 2 * x0 * x51 * x54 * x8 - x3 * x53 + x56 * x58 + x67 * x68 - x67; + *(out++) = 2 * curve_0 * curve_scale * x49 * x51 * x52 - x0 * x69 - x0 * x72 + x56 * x73 - x68 * x76 + x76; + *(out++) = x0 * x77 + x0 * x78 - x47 * x53 - x56 * x79 - x68 * x83 + x83; + *(out++) = x104 * x106 - x105 * x84 + x108 * x84 - x109 * x56 - x112 * x68 + x112; + *(out++) = x106 * x125 - x126 * x84 - x127 * x129 + x128 * x84 - x131 * x68 + x131; + *(out++) = -x106 * x141 + x129 * x143 + x142 * x84 - x144 * x84 - x146 * x68 + x146; + *(out++) = -x106 * x154 + x129 * x156 + x155 * x84 - x157 * x84 - x159 * x68 + x159; + *(out++) = 2 * curve_1 * curve_scale * x52 * x61 * x62 + x160 * x65 + x160 * x66 + x161 * x58 + x164 * x165 - x164; + *(out++) = -x110 * x160 * x45 + x160 * x52 * x62 * x70 + x161 * x73 - x165 * x167 - x166 * x59 + x167; + *(out++) = -x160 * x80 - x160 * x82 - x161 * x79 + x165 * x168 - x166 * x60 - x168; + *(out++) = -x107 * x171 - x109 * x161 - x111 * x169 - x111 * x170 + x165 * x172 - x172; + *(out++) = -x127 * x171 - x127 * x173 - x130 * x169 - x130 * x170 + x165 * x174 - x174; + *(out++) = x143 * x171 + x143 * x173 - x145 * x169 - x145 * x170 + x165 * x175 - x175; + *(out++) = x156 * x171 + x156 * x173 - x158 * x169 - x158 * x170 + x165 * x176 - x176; } - diff --git a/src/survive_vive.c b/src/survive_vive.c index 83bc977..0fae736 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -1233,10 +1233,10 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) if( ( ( type & 0xe8 ) == 0xe8 ) || doimu ) //Hmm, this looks kind of yucky... we can get e8's that are accelgyro's but, cleared by first propset. { propset |= 2; - FLT agm[9] = { 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + FLT agm[9] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; int j; - for( j = 0; j < 6; j++ ) - agm[j] = (int16_t)(readdata[j*2+1] | (readdata[j*2+2]<<8)); + for (j = 0; j < 6; j++) + agm[j] = (int16_t)(readdata[j * 2 + 1] | (readdata[j * 2 + 2] << 8)); calibrate_acc(w, agm); calibrate_gyro(w, agm+3); w->ctx->imuproc( w, 3, agm, (time1<<24)|(time2<<16)|readdata[0], 0 ); |