aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
authorJustin Berger <jdavidberger@gmail.com>2018-06-27 04:22:22 +0000
committerJustin Berger <jdavidberger@gmail.com>2018-06-27 04:22:22 +0000
commitcf36bd58b8c9d1c23ccfe1a52beafe376ba02e6b (patch)
treebae72f9ebe6937dbbabb7beec3115028a15743b0 /src
parent902bbde43fb6e000140da0da56c05f2d4af1df5f (diff)
parente68b69a9a14aeb70988c52666e60502fe6f049ab (diff)
downloadlibsurvive-cf36bd58b8c9d1c23ccfe1a52beafe376ba02e6b.tar.gz
libsurvive-cf36bd58b8c9d1c23ccfe1a52beafe376ba02e6b.tar.bz2
Merge remote-tracking branch 'origin/master' into fix_wired_tracker
Diffstat (limited to 'src')
-rw-r--r--src/poser_charlesrefine.c565
-rw-r--r--src/poser_general_optimizer.c13
-rw-r--r--src/poser_sba.c25
-rw-r--r--src/poser_sba.h16
-rw-r--r--src/survive.c97
-rwxr-xr-xsrc/survive_cal.c7
-rw-r--r--src/survive_config.c300
-rw-r--r--src/survive_config.h17
-rw-r--r--src/survive_default_devices.c23
-rw-r--r--src/survive_driver_dummy.c5
-rw-r--r--src/survive_playback.c12
-rwxr-xr-xsrc/survive_vive.c32
12 files changed, 730 insertions, 382 deletions
diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c
index b14d882..e95b621 100644
--- a/src/poser_charlesrefine.c
+++ b/src/poser_charlesrefine.c
@@ -1,5 +1,3 @@
-// Driver works, but you _must_ start it near the origin looking in +Z.
-
#include <poser.h>
#include <survive.h>
#include <survive_reproject.h>
@@ -15,25 +13,118 @@
#define MAX_PT_PER_SWEEP SENSORS_PER_OBJECT
+/*
+ More notes to self:
+ try updating expected lighthouse position against each lighthouse with imu data and don't tie them to each other.
+
+*/
+
+
+// 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 LIGHTCAP_DESCALE 0.2 //DO NOT EXCEED 0.7
+
+#define CORRECT_LATERAL_POSITION_COEFFICIENT LIGHTCAP_DESCALE // Explodes if you exceed 1.0 (Normally 0.7 for snappy non-IMU response)
+#define CORRECT_TELESCOPTION_COEFFICIENT (10.f*LIGHTCAP_DESCALE) // Converges even as high as 10.0 and doesn't explode. (Normally 7.0 for non-IMU respone)
+
+#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
+#define MINIMUM_CONFIDENCE_TO_CORRECT_POSITION 0.02 // 0.2
+#define POSE_CONFIDENCE_FOR_HANDLING_LINEAR_IMU .9 //0.9
+#define MAX_JUMP_DISTANCE 0.5 //0.4
+
+
+//Grand todo:
+// Update global "Up" vector from LH's PoV based on "Up" from the HMD.
+
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 quantity_errors[MAX_PT_PER_SWEEP];
+ FLT quantity_errors[MAX_PT_PER_SWEEP]; //Dot product of error offset.
FLT angles_at_pts[MAX_PT_PER_SWEEP];
SurvivePose object_pose_at_hit[MAX_PT_PER_SWEEP];
uint8_t sensor_ids[MAX_PT_PER_SWEEP];
LinmathPoint3d MixingPositions[NUM_LIGHTHOUSES][2];
- SurvivePose InteralPoseUsedForCalc; //Super high speed vibratey and terrible.
+ LinmathPoint3d mixed_output;
+
+ //Super high speed vibratey and terrible.
+ //Also, big deal: this does NOT have the "re-righting" vector
+ //from the accelerometer applied to it.
+ SurvivePose InteralPoseUsedForCalc;
+
FLT MixingConfidence[NUM_LIGHTHOUSES][2];
FLT last_angle_lh_axis[NUM_LIGHTHOUSES][2];
int ptsweep;
SurviveIMUTracker tracker;
+ SurvivePose * lastlhp;
+
+
+ //Additional flags, used when we start to try to use the accelerometer
+ LinmathPoint3d average_accelerometer_up_vector_in_world_space;
+ LinmathPoint3d velocity_according_to_accelerometer;
} CharlesPoserData;
+
+void AdjustRotation( SurviveObject *so, LinmathQuat adjustment, int is_imu, int is_coarse )
+{
+ CharlesPoserData *dd = so->PoserData;
+ //LinmathQuat invert_adjust;
+ //quatgetreciprocal( invert_adjust, adjustment );
+ quatrotateabout(dd->InteralPoseUsedForCalc.Rot, dd->InteralPoseUsedForCalc.Rot, adjustment );
+ quatnormalize(dd->InteralPoseUsedForCalc.Rot, dd->InteralPoseUsedForCalc.Rot);
+
+}
+
+void AdjustPosition( SurviveObject * so, LinmathPoint3d adjustment, int is_imu, float descale )
+{
+ CharlesPoserData *dd = so->PoserData;
+
+ add3d( dd->InteralPoseUsedForCalc.Pos, adjustment, dd->InteralPoseUsedForCalc.Pos);
+ add3d( dd->mixed_output, adjustment, dd->mixed_output);
+
+ if( descale > 0.0001 ) //Coming from lightcap.
+ {
+ LinmathPoint3d backflow;
+ scale3d( backflow, adjustment, 1.0/descale );
+ CharlesPoserData *dd = so->PoserData;
+ //XXX TODO figure out how to dampen velocity.
+ add3d( dd->velocity_according_to_accelerometer, dd->velocity_according_to_accelerometer, backflow );
+ scale3d( backflow, backflow, .001 );
+ add3d( dd->average_accelerometer_up_vector_in_world_space, dd->average_accelerometer_up_vector_in_world_space, backflow );
+ }
+
+}
+
+//Emits "dd->mixed_output" for position and dd->InteralPoseUsedForCalc.Rot for rotation.
+void EmitPose( SurviveObject *so, PoserData *pd )
+{
+ CharlesPoserData *dd = so->PoserData;
+
+ SurvivePose object_pose_out;
+ copy3d( object_pose_out.Pos, dd->mixed_output );
+
+ //average_accelerometer_up_vector_in_world_space should be "up"
+ LinmathVec3d true_up = { 0, 0, 1 };
+ LinmathVec3d dist_up;
+ normalize3d( dist_up, dd->average_accelerometer_up_vector_in_world_space ); //NOTE: Average vector probably won't be normalized.
+ LinmathQuat adjustment_from_rerighting_up;
+ quatfrom2vectors( adjustment_from_rerighting_up, dist_up, true_up );
+
+ quatcopy( object_pose_out.Rot, dd->InteralPoseUsedForCalc.Rot );
+ quatrotateabout( object_pose_out.Rot, adjustment_from_rerighting_up, dd->InteralPoseUsedForCalc.Rot );
+ PoserData_poser_pose_func(pd, so, &object_pose_out);
+}
+
+
+
int PoserCharlesRefine(SurviveObject *so, PoserData *pd) {
CharlesPoserData *dd = so->PoserData;
if (!dd)
@@ -42,57 +133,63 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) {
SurvivePose object_pose_out;
memcpy(&object_pose_out, &LinmathPose_Identity, sizeof(LinmathPose_Identity));
memcpy(&dd->InteralPoseUsedForCalc, &LinmathPose_Identity, sizeof(LinmathPose_Identity));
- so->PoseConfidence = 1.0;
+ so->PoseConfidence = 0.0;
PoserData_poser_pose_func(pd, so, &object_pose_out);
}
SurviveSensorActivations *scene = &so->activations;
switch (pd->pt) {
case POSERDATA_IMU: {
+ float imu_time = 1./ so->imu_freq;
// 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./1000.;
- imuData->gyro[1] *= 1./1000.;
- imuData->gyro[2] *= 1./1000.;
- 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 };
- LinmathVec3d normup;
- 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 );
+ {
+ LinmathQuat applygyro;
+ imuData->gyro[0] *= imu_time;
+ imuData->gyro[1] *= imu_time;
+ imuData->gyro[2] *= imu_time;
+ quatfromeuler( applygyro, imuData->gyro );
+ AdjustRotation( so, applygyro, 1, 0 );
+ }
+ //printf( "ACCEL %f %f %f\n", PFTHREE( imuData->accel ) );
- 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
- quatnormalize(object_pose_out.Rot, object_pose_out.Rot);
+ {
+ LinmathPoint3d rotated_out;
+ quatrotatevector( rotated_out, dd->InteralPoseUsedForCalc.Rot, imuData->accel );
+
+ if( so->PoseConfidence > 0.6 )
+ {
+ LinmathPoint3d correction;
+ //XXX Danger, will robinson.
+ //We are doing an IIR on the acceleration. Tests have shown THIS IS BAD. We should try to correct based on the lightcap data.
+ scale3d( dd->average_accelerometer_up_vector_in_world_space, dd->average_accelerometer_up_vector_in_world_space, .999 );
+ scale3d( correction, rotated_out, .001 );
+ add3d( dd->average_accelerometer_up_vector_in_world_space, dd->average_accelerometer_up_vector_in_world_space, correction );
- 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);
+ LinmathPoint3d deviation;
+ sub3d( deviation, rotated_out, dd->average_accelerometer_up_vector_in_world_space );
- //PoserDataIMU *imu = (PoserDataIMU *)pd;
- //survive_imu_tracker_integrate(so, &dd->tracker, imu);
- //PoserData_poser_pose_func(pd, so, &dd->tracker.pose);
+ LinmathPoint3d acc;
+ scale3d( acc, deviation, 9.8*imu_time );
+ add3d( dd->velocity_according_to_accelerometer, acc, dd->velocity_according_to_accelerometer );
+ scale3d( dd->velocity_according_to_accelerometer, dd->velocity_according_to_accelerometer, .999 ); //XXX Danger! We are doing an IIR on velocity. This is dangerous.
+ LinmathPoint3d posdiff;
+ scale3d( posdiff, dd->velocity_according_to_accelerometer, imu_time );
+ AdjustPosition( so, posdiff, 1, 0 );
+ }
+ else
+ {
+ copy3d( dd->average_accelerometer_up_vector_in_world_space, rotated_out );
+ scale3d( dd->velocity_according_to_accelerometer, dd->velocity_according_to_accelerometer, 0 );
+ }
+ }
+
+ EmitPose( so, pd );
return 0;
}
case POSERDATA_LIGHT: {
@@ -103,38 +200,25 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) {
BaseStationData *bsd = &so->ctx->bsd[ld->lh];
if (!bsd->PositionSet)
break;
- SurvivePose *lhp = &bsd->Pose;
+ SurvivePose *lhp = dd->lastlhp = &bsd->Pose;
FLT inangle = ld->angle;
int sensor_id = ld->sensor_id;
int axis = dd->sweepaxis;
- //const SurvivePose *object_pose = &so->OutPose;
dd->sweeplh = lhid;
- // 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_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.
ApplyPoseToPoint(sensor_position_worldspace, &dd->InteralPoseUsedForCalc, 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] );
- // = sensor position, relative to lighthouse center.
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.
FLT sweep_normal[3];
-
-
FLT inangles[2];
FLT outangles[2];
inangles[axis] = inangle;
@@ -195,15 +279,14 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) {
case POSERDATA_SYNC: {
PoserDataLight *l = (PoserDataLight *)pd;
int lhid = l->lh;
-
// you can get sweepaxis and sweeplh.
if (dd->ptsweep) {
int i;
+ int applied_corrections = 0;
+ int normal_faults = 0;
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.
BaseStationData *bsd = &so->ctx->bsd[lhid];
SurvivePose *lh_pose = &bsd->Pose;
@@ -211,141 +294,128 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) {
int validpoints = 0;
int ptvalid[MAX_PT_PER_SWEEP];
FLT avgerr = 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
-
-#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_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
-
- // Step 1: Determine standard of deviation, and average in order to
- // drop points that are likely in error.
{
- // Calculate average
- FLT avgerr_orig = 0.0;
- FLT stddevsq = 0.0;
- for (i = 0; i < pts; i++)
- avgerr_orig += dd->quantity_errors[i];
- avgerr_orig /= pts;
-
- // Calculate standard of deviation.
- for (i = 0; i < pts; i++) {
- FLT diff = dd->quantity_errors[i] - avgerr_orig;
- stddevsq += diff * diff;
- }
- stddevsq /= pts;
-
- for (i = 0; i < pts; i++) {
- FLT err = dd->quantity_errors[i];
- FLT diff = err - avgerr_orig;
- diff *= diff;
- int isptvalid = (diff * MIN_HIT_QUALITY <= stddevsq + HIT_QUALITY_BASELINE) ? 1 : 0;
- ptvalid[i] = isptvalid;
- if (isptvalid) {
- avgang += dd->angles_at_pts[i];
- avgerr += err;
- validpoints++;
+ FLT vec_correct[3] = {0., 0., 0.};
+ // Step 1: Determine standard of deviation, and average in order to
+ // drop points that are likely in error.
+ {
+ // Calculate average
+ FLT avgerr_orig = 0.0;
+ FLT stddevsq = 0.0;
+ for (i = 0; i < pts; i++)
+ avgerr_orig += dd->quantity_errors[i];
+ avgerr_orig /= pts;
+
+ // Calculate standard of deviation.
+ for (i = 0; i < pts; i++) {
+ FLT diff = dd->quantity_errors[i] - avgerr_orig;
+ stddevsq += diff * diff;
}
- }
- avgang /= validpoints;
- avgerr /= validpoints;
- }
-
- // 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 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;
+ stddevsq /= pts;
+
+ for (i = 0; i < pts; i++) {
+ FLT err = dd->quantity_errors[i];
+ FLT diff = err - avgerr_orig;
+ diff *= diff;
+ int isptvalid = (diff * MIN_HIT_QUALITY <= stddevsq + HIT_QUALITY_BASELINE) ? 1 : 0;
+ ptvalid[i] = isptvalid;
+ if (isptvalid) {
+ avgang += dd->angles_at_pts[i];
+ avgerr += err;
+ validpoints++;
+ }
+ }
+ avgang /= validpoints;
+ avgerr /= 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.
+ // Step 2: Determine average lateral error.
+ // We can actually always perform this operation. Even with only one point.
+ if ( so->PoseConfidence > MINIMUM_CONFIDENCE_TO_CORRECT_POSITION )
+ {
+ 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;
+ }
- // 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);
- }
+ // 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.
+ // 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.
- // 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.
- {
- FLT zoom = 0.0;
- FLT rmsang = 0.0;
- 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.
- zoom += delerr * delang;
- rmsang += delang * delang;
+ if( ( magnitude3d( avg_err ) < MAX_JUMP_DISTANCE || so->PoseConfidence < 0.8 ) )
+ {
+ scale3d(avg_err, avg_err, -CORRECT_LATERAL_POSITION_COEFFICIENT);
+ add3d(vec_correct, vec_correct, avg_err);
+ applied_corrections++;
+ }
+ else
+ {
+ so->PoseConfidence *= 0.9;
+ }
}
- // 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, dd->InteralPoseUsedForCalc.Pos);
- normalize3d(veccamalong, veccamalong);
- scale3d(veccamalong, veccamalong, zoom);
- add3d(vec_correct, veccamalong, vec_correct);
- }
-
-
-#if 0
- LinmathPoint3d LastDelta;
- sub3d( LastDelta, dd->MixingPositions[lhid][axis], dd->InteralPoseUsedForCalc.Pos );
- //Compare with "vec_correct"
-
- LinmathPoint3d DeltaDelta;
- sub3d( DeltaDelta, vec_correct, LastDelta );
-
-
- //SurvivePose object_pose_out;
-
- memcpy( dd->MixingPositions[lhid][axis], vec_correct, sizeof( vec_correct ) );
-
- LinmathPoint3d system_average_adjust = { 0, 0, 0 };
-
- printf( "%f %f %f + %f %f %f\n", vec_correct[0], vec_correct[1], vec_correct[2], dd->InteralPoseUsedForCalc.Pos[0], dd->InteralPoseUsedForCalc.Pos[1], dd->InteralPoseUsedForCalc.Pos[2] );
-
-#endif
- add3d(dd->InteralPoseUsedForCalc.Pos, vec_correct, dd->InteralPoseUsedForCalc.Pos);
+ // Step 3: Control telecoption from lighthouse.
+ // we need to find out what the weighting is to determine "zoom"
+ if (validpoints > 1 && so->PoseConfidence > MINIMUM_CONFIDENCE_TO_CORRECT_POSITION ) // 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;
+ 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.
+ zoom += delerr * delang;
+ rmsang += delang * delang;
+ }
- //quatcopy(object_pose_out.Rot, dd->InteralPoseUsedForCalc.Rot);
+ // 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;
+
+ //Don't apply completely wild zoom's unless our confidence is awful.
+ if( ( zoom < MAX_JUMP_DISTANCE || so->PoseConfidence < 0.8 ) )
+ {
+ FLT veccamalong[3];
+ sub3d(veccamalong, lh_pose->Pos, dd->InteralPoseUsedForCalc.Pos);
+ normalize3d(veccamalong, veccamalong);
+ scale3d(veccamalong, veccamalong, zoom);
+ add3d(vec_correct, veccamalong, vec_correct);
+ applied_corrections++;
+ }
+ else
+ {
+ so->PoseConfidence *= 0.9;
+ }
+ }
+ AdjustPosition( so, vec_correct, 0, (applied_corrections==2)?LIGHTCAP_DESCALE:0 );
+ }
// 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
@@ -356,71 +426,88 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) {
for (i = 0; i < pts; i++) {
if (!ptvalid[i])
continue;
- FLT dist = dd->quantity_errors[i] - avgerr;
+
+ FLT dist = dd->quantity_errors[i] - avgerr; //Relative dot-product error.
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];
+ FLT * sensornormal = &so->sensor_normals[sensor_id*3];
+ SurvivePose * lhp = dd->lastlhp;
+ 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.
-
- 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;
-
- // 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);
-
- 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);
-
- 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]);
+
+ //4A: First, check to see if this hit is a sensor that is facing the lighthouse.
+ //This is for coarse corrections early on in the calibration.
+ //If one of these happens it means the orientation/pose is totally impossible.
+ if( so->PoseConfidence < 0.9 ) {
+ LinmathPoint3d vector_to_lighthouse;
+ sub3d( vector_to_lighthouse, lhp->Pos, object_pose_at_hit->Pos ); //Get vector in world space.
+ normalize3d( vector_to_lighthouse, vector_to_lighthouse );
+ quatrotatevector( vector_to_lighthouse, world_to_object_space, vector_to_lighthouse );
+ float facingness = dot3d( sensornormal, vector_to_lighthouse );
+ if( facingness < -.1 )
+ {
+ //This is an impossible sensor hit.
+ so->PoseConfidence *= 0.8;
+
+ //If our pose confidence is low, apply a torque.
+ if( so->PoseConfidence < 0.8 )
+ {
+ LinmathPoint3d rotateaxis;
+ cross3d( rotateaxis, vector_to_lighthouse, sensornormal );
+ LinmathQuat correction;
+ quatfromaxisangle(correction, rotateaxis, facingness*.2 );
+ normal_faults ++;
+ AdjustRotation( so, correction, 0, 1 );
+ }
+ }
+ }
+
+ //Apply the normal tug.
+ {
+
+ 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);
+ dist *= CORRECT_ROTATION_COEFFICIENT;
+ 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.
+ FLT vector_from_center_of_object[3];
+ normalize3d(vector_from_center_of_object, sensor_inpos);
+ scale3d(vector_from_center_of_object, vector_from_center_of_object, 1);
+
+ FLT new_vector_in_object_space[3];
+ 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);
+
+ LinmathQuat corrective_quaternion;
+ quatfrom2vectors(corrective_quaternion, vector_from_center_of_object, new_vector_in_object_space);
+ quatrotateabout(correction, correction, corrective_quaternion);
+ }
+
}
- // printf( "Applying: %f %f %f %f\n", correction[0], correction[1], correction[2], correction[3] );
- // Apply our corrective quaternion to the output.
- quatrotateabout(dd->InteralPoseUsedForCalc.Rot, dd->InteralPoseUsedForCalc.Rot, correction);
- quatnormalize(dd->InteralPoseUsedForCalc.Rot, dd->InteralPoseUsedForCalc.Rot);
+ AdjustRotation( so, correction, 0, 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.
{
FLT MixedAmount = 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 };
@@ -431,31 +518,21 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) {
MixedAmount += Confidence;
//printf( "%f ", Confidence );
}
- scale3d( MixedPosition, MixedPosition, 1./MixedAmount );
-
- printf( "Reprojection disagreements:" );
- for( l = 0; l < NUM_LIGHTHOUSES; l++ ) for( a = 0; a < 2; a++ )
- {
- printf( "%f ", dist3d( dd->MixingPositions[l][a], MixedPosition ) );
- }
- printf( "\n" );
+ scale3d( dd->mixed_output, MixedPosition, 1./MixedAmount );
+ EmitPose( so, pd );
- //printf( "%f\n", MixedAmount );
- SurvivePose object_pose_out;
- 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);
dd->ptsweep = 0;
+
+ if( validpoints > 1 && applied_corrections > 1 && !normal_faults)
+ {
+ so->PoseConfidence += (1-so->PoseConfidence)*.04;
+ }
+ else if( validpoints > 1 && so->PoseConfidence < 0.5 && !normal_faults )
+ {
+ so->PoseConfidence += (1-so->PoseConfidence)*.01;
+ }
}
dd->sweepaxis = l->acode & 1;
diff --git a/src/poser_general_optimizer.c b/src/poser_general_optimizer.c
index 057ac3d..6f181a3 100644
--- a/src/poser_general_optimizer.c
+++ b/src/poser_general_optimizer.c
@@ -5,6 +5,11 @@
#include <malloc.h>
#include <stdio.h>
+STATIC_CONFIG_ITEM( CONFIG_MAX_ERROR, "max-error", 'f', "Maximum error permitted by poser_general_optimizer", .0001 );
+STATIC_CONFIG_ITEM( CONFIG_FAIL_TO_RESET, "failures-to-reset", 'i', "Failures needed before seed poser is re-run", 1 );
+STATIC_CONFIG_ITEM( CONFIG_SUC_TO_RESET, "successes-to-reset", 'i', "[TODO: I don't know what this does]", -1 );
+STATIC_CONFIG_ITEM( CONFIG_SEED_POSER, "seed-poser", 's', "Poser to be used to seed optimizer", "PoserEPNP" );
+
void *GetDriver(const char *name);
void general_optimizer_data_init(GeneralOptimizerData *d, SurviveObject *so) {
memset(d, 0, sizeof(*d));
@@ -12,11 +17,11 @@ void general_optimizer_data_init(GeneralOptimizerData *d, SurviveObject *so) {
SurviveContext *ctx = so->ctx;
- d->failures_to_reset = survive_configi(ctx, "failures-to-reset", SC_GET, 1);
- d->successes_to_reset = survive_configi(ctx, "successes-to-reset", SC_GET, -1);
- d->max_error = survive_configf(ctx, "max-error", SC_GET, .0001);
+ survive_attach_configf( ctx, "max-error", &d->max_error );
+ survive_attach_configi( ctx, "failures-to-reset", &d->failures_to_reset );
+ survive_attach_configi( ctx, "successes-to-reset", &d->successes_to_reset );
- const char *subposer = survive_configs(ctx, "seed-poser", SC_GET, "PoserEPNP");
+ const char *subposer = survive_configs(ctx, "seed-poser", SC_GET, 0 );
d->seed_poser = (PoserCB)GetDriver(subposer);
SV_INFO("Initializing general optimizer:");
diff --git a/src/poser_sba.c b/src/poser_sba.c
index ed19c1d..b049c80 100644
--- a/src/poser_sba.c
+++ b/src/poser_sba.c
@@ -20,6 +20,14 @@
#include "survive_kalman.h"
#include "survive_reproject.h"
+
+STATIC_CONFIG_ITEM( SBA_USE_IMU, "sba-use-imu", 'i', "[TODO: I don't know what this does]", 1 );
+STATIC_CONFIG_ITEM( SBA_REQUIRED_MEAS, "sba-required-meas", 'i', "[TODO: I don't know what this does]", 8 );
+STATIC_CONFIG_ITEM( SBA_TIME_WINDOW, "sba-time-window", 'i', "[TODO: I don't know what this does]", (int)SurviveSensorActivations_default_tolerance );
+STATIC_CONFIG_ITEM( SBA_SENSOR_VARIANCE_PER_SEC, "sba-sensor-variance-per-sec", 'f', "[TODO: I don't know what this does]", 10.0 );
+STATIC_CONFIG_ITEM( SBA_SENSOR_VARIANCE, "sba-sensor-variance", 'f', "[TODO: I don't know what this does]", 1.0 );
+STATIC_CONFIG_ITEM( SBA_USE_JACOBIAN_FUNCTION, "sba-use-jacobian-function", 'i', "Poser to be used to seed optimizer", 1);
+
typedef struct {
PoserData *pdfs;
SurviveObject *so;
@@ -42,7 +50,7 @@ typedef struct SBAData {
survive_kpose_t kpose;
SurviveIMUTracker tracker;
- bool useIMU;
+ int useIMU;
struct {
int meas_failures;
@@ -343,14 +351,13 @@ int PoserSBA(SurviveObject *so, PoserData *pd) {
SBAData *d = so->PoserData;
general_optimizer_data_init(&d->opt, so);
- d->useIMU = survive_configi(ctx, "sba-use-imu", SC_GET, 1);
- d->required_meas = survive_configi(ctx, "sba-required-meas", SC_GET, 8);
-
- 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, 10.0);
- 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);
+
+ survive_attach_configi( ctx, "sba-use-imu", &d->useIMU );
+ survive_attach_configi( ctx, "sba-required-meas", &d->required_meas );
+ survive_attach_configi( ctx, "sba-time-window", &d->sensor_time_window );
+ survive_attach_configf( ctx, "sba-sensor-variance-per-sec", &d->sensor_variance_per_second );
+ survive_attach_configf( ctx, "sba-sensor-variance", &d->sensor_variance );
+ survive_attach_configi( ctx, "sba-use-jacobian-function", &d->use_jacobian_function );
SV_INFO("Initializing SBA:");
SV_INFO("\tsba-required-meas: %d", d->required_meas);
diff --git a/src/poser_sba.h b/src/poser_sba.h
deleted file mode 100644
index 8a49122..0000000
--- a/src/poser_sba.h
+++ /dev/null
@@ -1,16 +0,0 @@
-#pragma once
-
-#pragma once
-#include <stdint.h>
-#define FLT double
-#include "survive_reproject.h"
-#include <libsurvive/poser.h>
-#include <ostream>
-
-struct SurviveObject;
-
-int sba_bruteforce_config_solver_cb(SurviveObject *so, PoserData *pd);
-int sba_solver_poser_cb(SurviveObject *so, PoserData *pd);
-
-std::ostream &operator<<(std::ostream &o, const survive_calibration_options_config &self);
-std::ostream &operator<<(std::ostream &o, const survive_calibration_config &self); \ No newline at end of file
diff --git a/src/survive.c b/src/survive.c
index 20a2a1b..920b9ec 100644
--- a/src/survive.c
+++ b/src/survive.c
@@ -16,6 +16,11 @@
#define z_const const
#endif
+STATIC_CONFIG_ITEM( BLACKLIST_DEVS, "blacklist-devs", 's', "List any devs (or substrings of devs) to blacklist.", "-" );
+STATIC_CONFIG_ITEM( CONFIG_FILE, "configfile", 's', "Default configuration file", "config.json" );
+STATIC_CONFIG_ITEM( CONFIG_D_CALI, "disable-calibrate", 'i', "Enables or disables calibration", 0 );
+STATIC_CONFIG_ITEM( CONFIG_F_CALI, "force-calibrate", 'i', "Forces calibration even if one exists.", 0 );
+
#ifdef RUNTIME_SYMNUM
#include <symbol_enumerator.h>
static int did_runtime_symnum;
@@ -93,6 +98,20 @@ void survive_verify_FLT_size(uint32_t user_size) {
}
}
+static void PrintMatchingDrivers( const char * prefix, const char * matchingparam )
+{
+ int i = 0;
+ char stringmatch[128];
+ snprintf( stringmatch, 127, "%s%s", prefix, matchingparam?matchingparam:"" );
+ const char * DriverName;
+ while ((DriverName = GetDriverNameMatching(stringmatch, i++)))
+ {
+ printf( "%s ", DriverName+strlen(prefix) );
+ }
+}
+
+
+
SurviveContext *survive_init_internal(int argc, char *const *argv) {
int i;
@@ -133,21 +152,21 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) {
ctx->state = SURVIVE_STOPPED;
- survive_config_populate_ctx( ctx );
-
ctx->global_config_values = malloc(sizeof(config_group));
ctx->temporary_config_values = malloc(sizeof(config_group));
ctx->lh_config = malloc(sizeof(config_group) * NUM_LIGHTHOUSES);
// initdata
- init_config_group(ctx->global_config_values, 10, ctx);
- init_config_group(ctx->temporary_config_values, 20, ctx);
+ init_config_group(ctx->global_config_values, 30, ctx);
+ init_config_group(ctx->temporary_config_values, 30, ctx);
for( i = 0; i < NUM_LIGHTHOUSES; i++ )
init_config_group(&ctx->lh_config[i], 10, ctx);
// Process command-line parameters.
char *const *av = argv + 1;
char *const *argvend = argv + argc;
+ int list_for_autocomplete = 0;
+ const char * autocomplete_match[3] = { 0, 0, 0};
int showhelp = 0;
for (; av < argvend; av++) {
if ((*av)[0] != '-')
@@ -168,6 +187,13 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) {
case 'l':
vartoupdate = "lighthousecount";
break;
+ case 'm':
+ if( av + 1 < argvend ) autocomplete_match[0] = *(av+1);
+ if( av + 2 < argvend ) autocomplete_match[1] = *(av+2);
+ if( av + 3 < argvend ) autocomplete_match[2] = *(av+3);
+ list_for_autocomplete = 1;
+ av = argvend; //Eject immediately after processing -m
+ break;
case 'c':
vartoupdate = "configfile";
break;
@@ -196,33 +222,54 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) {
}
}
}
- if (showhelp) {
- // Can't use SV_ERROR here since we don't have a context to send to yet.
- fprintf(stderr, "libsurvive - usage:\n");
- fprintf(stderr, " --[parameter] [value] - sets parameter\n");
- fprintf(stderr, " -h - shows help.\n");
- fprintf(stderr, " -p [poser] - use a specific defaultposer.\n");
- fprintf(stderr, " -l [lighthouse count] - use a specific number of lighthoses.\n");
- fprintf(stderr, " -c [config file] - set config file\n");
- fprintf(stderr, " --record [log file] - Write all events to the given record file.\n");
- fprintf(stderr, " --playback [log file] - Read events from the given file instead of USB devices.\n");
- fprintf(stderr, " --playback-factor [f] - Time factor of playback -- 1 is run at the same timing as "
- "original, 0 is run as fast as possible.\n");
- }
-
config_read(ctx, survive_configs(ctx, "configfile", SC_GET, "config.json"));
ctx->activeLighthouses = survive_configi(ctx, "lighthousecount", SC_SETCONFIG, 2);
config_read_lighthouse(ctx->lh_config, &(ctx->bsd[0]), 0);
config_read_lighthouse(ctx->lh_config, &(ctx->bsd[1]), 1);
+ ctx->faultfunction = survivefault;
+ ctx->notefunction = survivenote;
+
+ if( list_for_autocomplete )
+ {
+ const char * lastparam = (autocomplete_match[2]==0)?autocomplete_match[1]:autocomplete_match[2];
+ const char * matchingparam = (autocomplete_match[2]==0)?0:autocomplete_match[1];
+ //First see if any of the parameters perfectly match a config item, if so print some help.
+ //fprintf( stderr, "!!! %s !!! %s !!!\n", lastparam, matchingparam );
+
+ const char * checkconfig = matchingparam;
+ if( matchingparam == 0 ) checkconfig = lastparam;
+
+ if( checkconfig && strlen( checkconfig ) > 2 && survive_print_help_for_parameter( checkconfig+2 ) )
+ {
+ exit(0);
+ }
+
+ if( strstr( lastparam, "poser" ) ) PrintMatchingDrivers( "Poser", matchingparam );
+ else if( strstr( lastparam, "disambiguator" ) ) PrintMatchingDrivers( "Disambiguator", matchingparam );
+ else
+ {
+ printf( "-h -m -p -l -c " );
+ survive_print_known_configs( ctx, 0 );
+ }
+ printf( "\n" );
+ exit( 0 );
+ }
+
if( showhelp )
{
- survive_print_known_configs( ctx );
+ // Can't use SV_ERROR here since we don't have a context to send to yet.
+ fprintf(stderr, "libsurvive - usage:\n");
+ fprintf(stderr, " -h - shows help.\n");
+ fprintf(stderr, " -m - list parameters, for autocomplete." );
+ fprintf(stderr, " -p [poser] - use a specific defaultposer.\n");
+ fprintf(stderr, " -l [lighthouse count] - use a specific number of lighthoses.\n");
+ fprintf(stderr, " -c [config file] - set config file\n");
+ fprintf(stderr, "Additional --[parameter] [value] - sets generic parameters...\n");
+ survive_print_known_configs( ctx, 1 );
return 0;
}
- ctx->faultfunction = survivefault;
- ctx->notefunction = survivenote;
ctx->lightproc = survive_default_light_process;
ctx->imuproc = survive_default_imu_process;
ctx->angleproc = survive_default_angle_process;
@@ -319,10 +366,10 @@ int survive_startup(SurviveContext *ctx) {
ctx->state = SURVIVE_RUNNING;
- int calibrateMandatory = survive_configi(ctx, "calibrate", SC_GET, 0);
- int calibrateForbidden = survive_configi(ctx, "calibrate", SC_GET, 1) == 0;
+ int calibrateMandatory = survive_configi(ctx, "force-calibrate", SC_GET, 0);
+ int calibrateForbidden = survive_configi(ctx, "disable-calibrate", SC_GET, 1) == 0;
if (calibrateMandatory && calibrateForbidden) {
- SV_INFO("Contradictory settings --calibrate and --no-calibrate specified. Switching to normal behavior.");
+ SV_INFO("Contradictory settings --force-calibrate and --disable-calibrate specified. Switching to normal behavior.");
calibrateMandatory = calibrateForbidden = 0;
}
@@ -334,7 +381,7 @@ int survive_startup(SurviveContext *ctx) {
if (!isCalibrated) {
SV_INFO("Uncalibrated configuration detected. Attaching calibration. Please don't move tracked objects for "
- "the duration of calibration. Pass '--no-calibrate' to skip calibration");
+ "the duration of calibration. Pass '--disable-calibrate' to skip calibration");
} else {
SV_INFO("Calibration requested. Previous calibration will be overwritten.");
}
diff --git a/src/survive_cal.c b/src/survive_cal.c
index f809809..f270065 100755
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -27,6 +27,11 @@
int mkdir(const char *);
#endif
+
+STATIC_CONFIG_ITEM( REQ_TRACK_FOR_CAL, "requiredtrackersforcal", 's', "Which devices will be used, i.e. HMD,WM0,WM1", "" );
+STATIC_CONFIG_ITEM( ALLOW_TRACK_FOR_CAL, "allowalltrackersforcal", 'i', "Allow use of additional connected devices for calibration", 0 );
+STATIC_CONFIG_ITEM( CONFIG_POSER, "configposer", 's', "Poser used for calibration step", "SBA" );
+
#define PTS_BEFORE_COMMON 32
#define NEEDED_COMMON_POINTS 10
#define MIN_SENSORS_VISIBLE_PER_LH_FOR_CAL 4
@@ -150,7 +155,7 @@ void survive_cal_install( struct SurviveContext * ctx )
// basically, libsurvive will detect whatever they have plugged in and start using that.
// const char * RequiredTrackersForCal = config_read_str(ctx->global_config_values, "RequiredTrackersForCal", "HMD,WM0,WM1");
const char *RequiredTrackersForCal = survive_configs(ctx, "requiredtrackersforcal", SC_SETCONFIG, "");
- const uint32_t AllowAllTrackersForCal = survive_configi(ctx, "allowalltrackersforcal", SC_SETCONFIG, 1);
+ const uint32_t AllowAllTrackersForCal = survive_configi(ctx, "allowalltrackersforcal", SC_SETCONFIG, 0);
size_t requiredTrackersFound = 0;
for (int j=0; j < ctx->objs_ct; j++)
diff --git a/src/survive_config.c b/src/survive_config.c
index 92c0125..5ee4a84 100644
--- a/src/survive_config.c
+++ b/src/survive_config.c
@@ -28,28 +28,32 @@ struct static_conf_t
};
static struct static_conf_t static_configs[MAX_SHORTHAND_CONFIGS];
+int nr_static_configs;
-void survive_config_bind_variable( char vt, int * variable, const char * name, const char * description, ... )
+void survive_config_bind_variable( char vt, const char * name, const char * description, ... )
{
va_list ap;
va_start(ap, description);
int i;
struct static_conf_t * config;
- for( i = 0; i < MAX_SHORTHAND_CONFIGS; i++ )
+ for( i = 0; i < nr_static_configs; i++ )
{
config = &static_configs[i];
- if( !config->name || strcmp( config->name, name ) == 0 ) break;
+ if( strcmp( config->name, name ) == 0 ) break;
}
+ config = &static_configs[i];
if( i == MAX_SHORTHAND_CONFIGS )
{
fprintf( stderr, "Fatal: Too many static configuration items. Please recompile with a higher MAX_STATIC_CONFIGS\n" );
exit( -1 );
}
+ if( i == nr_static_configs ) nr_static_configs++;
+
if( !config->description ) config->description = description;
if( !config->name ) config->name = name;
if( config->type && config->type != vt )
{
- fprintf( stderr, "Fatal: Internal error on variable %s. Types disagree.\n", name );
+ fprintf( stderr, "Fatal: Internal error on variable %s. Types disagree [%c/%c].\n", name, config->type, vt );
exit( -2 );
}
config->type = vt;
@@ -61,50 +65,113 @@ void survive_config_bind_variable( char vt, int * variable, const char * name, c
default:
fprintf( stderr, "Fatal: Internal error on variable %s. Unknown type %c\n", name, vt );
}
- *variable = i;
}
-void survive_config_populate_ctx( SurviveContext * ctx )
+int survive_print_help_for_parameter( const char * tomap )
{
int i;
- struct static_conf_t * config;
- for( i = 0; i < MAX_SHORTHAND_CONFIGS; i++ )
+ for( i = 0; i < nr_static_configs; i++ )
{
- config = &static_configs[i];
- switch( config->type )
+ struct static_conf_t * sc = &static_configs[i];
+ if( strcmp( sc->name, tomap ) == 0 )
{
- case 'i': ctx->shorthand_configs[i].i = config->data_default.i;
- case 'f': ctx->shorthand_configs[i].f = config->data_default.f;
- case 's': ctx->shorthand_configs[i].s = config->data_default.s;
+ char sthelp[160];
+ snprintf( sthelp, 159, " %s: %s [%c]", sc->name, sc->description, sc->type );
+ fprintf( stderr, "\0337\033[1A\033[1000D%s\0338", sthelp );
+ return 1;
}
}
+ return 0;
}
-void survive_print_known_configs( SurviveContext * ctx )
+static void PrintConfigGroup(config_group * grp, const char ** chkval, int * cvs, int verbose )
{
- int i;
- struct static_conf_t * config;
- for( i = 0; i < MAX_SHORTHAND_CONFIGS; i++ )
+ int i, j;
+ for( i = 0; i < grp->used_entries; i++ )
{
- config = &static_configs[i];
- if( !config->name ) break;
- switch( config->type )
+ config_entry * ce = &grp->config_entries[i];
+ for( j = 0; j < *cvs; j++ )
{
- case 'i': printf( "%10d %20s %s\n", config->data_default.i, config->name, config->description ); break;
- case 'f': printf( "%10f %20s %s\n", config->data_default.f, config->name, config->description ); break;
- case 's': printf( "%10s %20s %s\n", config->data_default.s, config->name, config->description ); break;
+ if( strcmp( chkval[j], ce->tag ) == 0 ) break;
+ }
+ if( j == *cvs )
+ {
+ if( verbose )
+ {
+ char stobuf[128];
+ switch( ce->type )
+ {
+ case CONFIG_UINT32: snprintf( stobuf, 127, " --%s %d", ce->tag, ce->numeric.i ); break;
+ case CONFIG_FLOAT: snprintf( stobuf, 127, " --%s %f", ce->tag, ce->numeric.f ); break;
+ case CONFIG_STRING: snprintf( stobuf, 127, " --%s %s", ce->tag, ce->data ); break;
+ case CONFIG_FLOAT_ARRAY: printf( "[FA] %20s", ce->tag ); break;
+ }
+
+ int namepad = 40 - strlen( stobuf );
+ printf( "%s%*c", stobuf, namepad, (ce->type==CONFIG_FLOAT)?'f':(ce->type==CONFIG_UINT32)?'i':(ce->type==CONFIG_STRING)?'s':'.' );
+
+ //Try to get description from the static tags.
+ for( j = 0; j < nr_static_configs; j++ )
+ {
+ if( strcmp( static_configs[j].name, ce->tag ) == 0 )
+ {
+ printf( " %s", static_configs[j].description );
+ }
+ }
+ printf( "\n" );
+ }
+ else
+ {
+ printf( "--%s ", ce->tag );
+ }
+ chkval[*cvs] = ce->tag;
+ (*cvs)++;
}
}
-
- //XXX TODO: Maybe this should print out the actual config values after being updated from the rest of the config system?
- //struct config_group *global_config_values;
- //struct config_group *temporary_config_values; // Set per-session, from command-line. Not saved but override global_config_values
}
+void survive_print_known_configs( SurviveContext * ctx, int verbose )
+{
+ int i;
-
-
-
+ const char * checked_values[256];
+ int cvs = 0;
+ memset( checked_values, 0, sizeof( checked_values ) );
+ PrintConfigGroup( ctx->temporary_config_values, checked_values, &cvs, verbose );
+ PrintConfigGroup( ctx->global_config_values, checked_values, &cvs, verbose );
+ int j;
+ for( j = 0; j < nr_static_configs; j++ )
+ {
+ for( i = 0; i < cvs; i++ )
+ {
+ if( strcmp( static_configs[j].name, checked_values[i] ) == 0 )
+ break;
+ }
+ if( i == cvs )
+ {
+ struct static_conf_t * config = &static_configs[j];
+ const char * name = config->name;
+
+ if( verbose )
+ {
+ char stobuf[128];
+ switch( config->type )
+ {
+ case 'i': snprintf( stobuf, 127, " --%s %d", name, config->data_default.i ); break;
+ case 'f': snprintf( stobuf, 127, " --%s %f", name, config->data_default.f ); break;
+ case 's': snprintf( stobuf, 127, " --%s %s", name, config->data_default.s ); break;
+ case 'a': snprintf( stobuf, 127, "[FA] %25s %s\n", config->name, config->description ); break;
+ }
+ int namepad = 40 - strlen( stobuf );
+ printf( "%s%*c %s\n", stobuf, namepad, config->type, config->description );
+ }
+ else
+ {
+ printf( "--%s ", name );
+ }
+ }
+ }
+}
const FLT *config_set_float_a(config_group *cg, const char *tag, const FLT *values, uint8_t count);
@@ -113,7 +180,7 @@ void init_config_entry(config_entry *ce) {
ce->tag = NULL;
ce->type = CONFIG_UNKNOWN;
ce->elements = 0;
- ce->shorthand_place = -1;
+ ce->update_list = 0;
}
void destroy_config_entry(config_entry *ce) {
@@ -313,20 +380,6 @@ config_entry *next_unused_entry(config_group *cg, const char * tag) {
cg->used_entries++;
-
- int i;
- struct static_conf_t * config;
- for( i = 0; i < MAX_SHORTHAND_CONFIGS; i++ )
- {
- config = &static_configs[i];
- if( !config->name ) break;
- if( strcmp( config->name, tag ) == 0 )
- {
- cv->shorthand_place = i;
- break;
- }
- }
-
return cv;
}
@@ -344,7 +397,8 @@ const char *config_set_str(config_group *cg, const char *tag, const char *value)
}
cv->type = CONFIG_STRING;
- if( cv->shorthand_place >= 0 ) cg->ctx->shorthand_configs[cv->shorthand_place].s = value;
+ update_list_t * t = cv->update_list;
+ while( t ) { *((const char **)t->value) = value; t = t->next; }
return value;
}
@@ -358,7 +412,8 @@ const uint32_t config_set_uint32(config_group *cg, const char *tag, const uint32
cv->numeric.i = value;
cv->type = CONFIG_UINT32;
- if( cv->shorthand_place >= 0 ) cg->ctx->shorthand_configs[cv->shorthand_place].i = value;
+ update_list_t * t = cv->update_list;
+ while( t ) { *((uint32_t*)t->value) = value; t = t->next; }
return value;
}
@@ -372,7 +427,9 @@ const FLT config_set_float(config_group *cg, const char *tag, const FLT value) {
cv->numeric.f = value;
cv->type = CONFIG_FLOAT;
- if( cv->shorthand_place >= 0 ) cg->ctx->shorthand_configs[cv->shorthand_place].f = value;
+
+ update_list_t * t = cv->update_list;
+ while( t ) { *((FLT*)t->value) = value; t = t->next; }
return value;
}
@@ -610,6 +667,21 @@ FLT survive_configf(SurviveContext *ctx, const char *tag, char flags, FLT def) {
}
}
+
+ int i;
+ if( !(flags & SC_OVERRIDE) )
+ {
+ for( i = 0; i < nr_static_configs; i++ )
+ {
+ struct static_conf_t * sc = &static_configs[i];
+ if( strcmp( tag, sc->name ) == 0 )
+ {
+ def = sc->data_default.f;
+ }
+ }
+ }
+
+
// If override is flagged, or, we can't find the variable, ,continue on.
if (flags & SC_SETCONFIG) {
config_set_float(ctx->temporary_config_values, tag, def);
@@ -629,6 +701,21 @@ uint32_t survive_configi(SurviveContext *ctx, const char *tag, char flags, uint3
}
}
+ uint32_t statictimedef = def;
+ int i;
+ if( !(flags & SC_OVERRIDE) )
+ {
+ for( i = 0; i < nr_static_configs; i++ )
+ {
+ struct static_conf_t * sc = &static_configs[i];
+ if( strcmp( tag, sc->name ) == 0 )
+ {
+ def = sc->data_default.i;
+ }
+ }
+ }
+
+
// If override is flagged, or, we can't find the variable, ,continue on.
if (flags & SC_SETCONFIG) {
config_set_uint32(ctx->temporary_config_values, tag, def);
@@ -647,15 +734,120 @@ const char *survive_configs(SurviveContext *ctx, const char *tag, char flags, co
return cv->data;
}
- // If override is flagged, or, we can't find the variable, ,continue on.
- if (flags & SC_SETCONFIG) {
- config_set_str(ctx->temporary_config_values, tag, def);
- config_set_str(ctx->global_config_values, tag, def);
- } else if (flags & SC_SET) {
- config_set_str(ctx->temporary_config_values, tag, def);
+ int i;
+ char foundtype = 0;
+ const char * founddata = def;
+ for( i = 0; i < nr_static_configs; i++ )
+ {
+ struct static_conf_t * sc = &static_configs[i];
+ if( !sc ) break;
+ if( strcmp( tag, sc->name ) == 0 )
+ {
+ founddata = sc->data_default.s;
+ foundtype = sc->type;
+ if( !(flags & SC_OVERRIDE) )
+ {
+ def = founddata;
+ }
+ }
+ }
+
+ if( !foundtype || foundtype == 's' )
+ {
+ // If override is flagged, or, we can't find the variable, ,continue on.
+ if (flags & SC_SETCONFIG) {
+ config_set_str(ctx->temporary_config_values, tag, def);
+ config_set_str(ctx->global_config_values, tag, def);
+ } else if (flags & SC_SET) {
+ config_set_str(ctx->temporary_config_values, tag, def);
+ } else {
+ return founddata;
+ }
+ }
+ else if( foundtype == 'i' )
+ {
+ survive_configi( ctx, tag, flags, atoi( def ) );
+ }
+ else if( foundtype == 'f' )
+ {
+ survive_configf( ctx, tag, flags, atof( def ) );
}
return def;
}
+static void survive_attach_config(SurviveContext *ctx, const char *tag, void * var, char type )
+{
+ config_entry *cv = sc_search(ctx, tag);
+ if( !cv )
+ {
+ //Check to see if there is a static config with this value.
+ if( type == 'i' )
+ {
+ *((int*)var) = survive_configi( ctx, tag, SC_SET, 0);
+ }
+ if( type == 'f' )
+ {
+ *((FLT*)var) = survive_configf( ctx, tag, SC_SET, 0);
+ }
+ if( type == 's' )
+ {
+ const char * cv = survive_configs( ctx, tag, SC_SET, 0);
+ memcpy( var, cv, strlen(cv) );
+ }
+ cv = sc_search(ctx, tag);
+ if( !cv )
+ {
+ SV_ERROR( "Configuration item %s not initialized.\n", tag );
+ return;
+ }
+ }
+ update_list_t ** ul = &cv->update_list;
+ while( *ul )
+ {
+ if( (*ul)->value == var ) return;
+ ul = &((*ul)->next);
+ }
+ update_list_t * t = *ul = malloc( sizeof( update_list_t ) );
+ t->next = 0;
+ t->value = var;
+}
+
+SURVIVE_EXPORT void survive_attach_configi(SurviveContext *ctx, const char *tag, int * var )
+{
+ survive_attach_config( ctx, tag, var, 'i' );
+}
+
+SURVIVE_EXPORT void survive_attach_configf(SurviveContext *ctx, const char *tag, FLT * var )
+{
+ survive_attach_config( ctx, tag, var, 'f' );
+}
+
+SURVIVE_EXPORT void survive_attach_configs(SurviveContext *ctx, const char *tag, char * var )
+{
+ survive_attach_config( ctx, tag, var, 's' );
+}
+
+SURVIVE_EXPORT void survive_detach_config(SurviveContext *ctx, const char *tag, void * var )
+{
+ config_entry *cv = sc_search(ctx, tag);
+ if( !cv )
+ {
+ SV_ERROR( "Configuration item %s not initialized.\n", tag );
+ return;
+ }
+
+ update_list_t ** ul = &cv->update_list;
+ while( *ul )
+ {
+ if( (*ul)->value == var )
+ {
+ update_list_t * v = *ul;
+ *ul = (*ul)->next;
+ free( v );
+ }
+ ul = &((*ul)->next);
+ }
+}
+
diff --git a/src/survive_config.h b/src/survive_config.h
index 37e904c..3f823b0 100644
--- a/src/survive_config.h
+++ b/src/survive_config.h
@@ -16,6 +16,14 @@ typedef enum {
CONFIG_FLOAT_ARRAY = 4,
} cval_type;
+struct update_list_t_s
+{
+ void * value;
+ struct update_list_t_s * next;
+};
+
+typedef struct update_list_t_s update_list_t;
+
typedef struct {
char *tag;
@@ -26,7 +34,8 @@ typedef struct {
} numeric;
char *data;
uint32_t elements;
- int shorthand_place;
+
+ update_list_t * update_list;
} config_entry;
typedef struct config_group {
@@ -63,8 +72,10 @@ uint32_t config_read_uint32(config_group *cg, const char *tag, const uint32_t de
const char* config_read_str(config_group *cg, const char *tag, const char *def);
//These are for the internal non-function configuration system.
-void survive_config_bind_variable( char vt, int * variable, const char * name, const char * description, ... );
-void survive_print_known_configs();
+void survive_config_bind_variable( char vt, const char * name, const char * description, ... );
+void survive_print_known_configs( SurviveContext * ctx, int verbose );
void survive_config_populate_ctx( SurviveContext * ctx );
+int survive_print_help_for_parameter( const char * tomap );
+
#endif
diff --git a/src/survive_default_devices.c b/src/survive_default_devices.c
index 48f3b41..8ecff0b 100644
--- a/src/survive_default_devices.c
+++ b/src/survive_default_devices.c
@@ -6,6 +6,9 @@
#include "json_helpers.h"
+#define HMD_IMU_HZ 1000.0f
+#define VIVE_DEFAULT_IMU_HZ 250.0f
+
static SurviveObject *
survive_create_device(SurviveContext *ctx, const char *driver_name,
void *driver, const char *device_name, haptic_func fn) {
@@ -17,7 +20,7 @@ survive_create_device(SurviveContext *ctx, const char *driver_name,
memcpy(device->drivername, driver_name, strlen(driver_name));
device->timebase_hz = 48000000;
- device->imu_freq = 250.f;
+ device->imu_freq = VIVE_DEFAULT_IMU_HZ;
device->haptic = fn;
return device;
@@ -171,21 +174,27 @@ int survive_load_htc_config_format(SurviveObject *so, char *ct0conf, int len) {
if (strcmp(so->codename, "HMD") == 0) {
if (so->acc_scale) {
scale3d(so->acc_scale, so->acc_scale, 1. / 8192.0);
+ //Invert X and Y at least on my vive.
+ so->acc_scale[1] *= -1;
+ so->acc_scale[0] *= -1;
}
if (so->acc_bias)
- scale3d(so->acc_bias, so->acc_bias, 2. / 1000.); // Odd but seems right.
+ scale3d(so->acc_bias, so->acc_bias, 1000.0 ); // Odd but seems right.
+
+ so->imu_freq = HMD_IMU_HZ;
+
if (so->gyro_scale) {
FLT deg_per_sec = 500;
scale3d(so->gyro_scale, so->gyro_scale, deg_per_sec / (1 << 15) * LINMATHPI / 180.);
- // scale3d(so->gyro_scale, so->gyro_scale, 0.000065665);
+ //Invert X and Y at least on my vive.
+ so->gyro_scale[0] *= -1;
+ so->gyro_scale[1] *= -1;
}
-
- so->imu_freq = 1000.f;
} 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.
+ scale3d(so->acc_bias, so->acc_bias, 1000.); // Need to verify.
FLT deg_per_sec = 2000;
if (so->gyro_scale)
@@ -206,7 +215,7 @@ int survive_load_htc_config_format(SurviveObject *so, char *ct0conf, int len) {
// 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.);
+ scale3d(so->acc_bias, so->acc_bias, 1000.);
// From datasheet, can be 250, 500, 1000, 2000 deg/s range over 16 bits
FLT deg_per_sec = 2000;
diff --git a/src/survive_driver_dummy.c b/src/survive_driver_dummy.c
index 01d0482..e305b33 100644
--- a/src/survive_driver_dummy.c
+++ b/src/survive_driver_dummy.c
@@ -8,7 +8,7 @@
#include "survive_config.h"
#include "os_generic.h"
-STATIC_CONFIG_ITEM_I( DUMMY_DRIVER_ENABLE, "dummy-driver-enable", "Load a dummy driver for testing.", 0 );
+STATIC_CONFIG_ITEM( DUMMY_DRIVER_ENABLE, "dummy-driver-enable", 'i', "Load a dummy driver for testing.", 0 );
struct SurviveDriverDummy {
SurviveContext * ctx;
@@ -55,7 +55,8 @@ int dummy_haptic( SurviveObject * so, uint8_t reserved, uint16_t pulseHigh, uint
int DriverRegDummy(SurviveContext *ctx)
{
- if( !GCONFIGI(DUMMY_DRIVER_ENABLE) ) return 0;
+ int enable = survive_configi( ctx, "dummy-driver-enable", SC_GET, 9 );
+ if( !enable ) return 0;
SurviveDriverDummy *sp = calloc(1, sizeof(SurviveDriverDummy));
sp->ctx = ctx;
diff --git a/src/survive_playback.c b/src/survive_playback.c
index 50b2e5c..c616aea 100644
--- a/src/survive_playback.c
+++ b/src/survive_playback.c
@@ -21,8 +21,9 @@ ssize_t getdelim(char **lineptr, size_t *n, int delimiter, FILE *stream);
ssize_t getline(char **lineptr, size_t *n, FILE *stream);
#endif
-
-STATIC_CONFIG_ITEM_F( PLAYBACK_FACTOR, "playback-factor", "Time factor of playback -- 1 is run at the same timing as original, 0 is run as fast as possible.", 1.0f );
+STATIC_CONFIG_ITEM( RECORD, "record", 's', "File to record to if you wish to make a recording.", "" );
+STATIC_CONFIG_ITEM( PLAYBACK, "playback", 's', "File to be used for playback if playing a recording.", "" );
+STATIC_CONFIG_ITEM( PLAYBACK_FACTOR, "playback-factor", 'f', "Time factor of playback -- 1 is run at the same timing as original, 0 is run as fast as possible.", 1.0f );
typedef struct SurviveRecordingData {
@@ -178,6 +179,7 @@ struct SurvivePlaybackData {
int lineno;
double next_time_us;
+ FLT playback_factor;
bool hasRawLight;
};
typedef struct SurvivePlaybackData SurvivePlaybackData;
@@ -300,7 +302,7 @@ static int playback_poll(struct SurviveContext *ctx, void *_driver) {
line = 0;
}
- if (driver->next_time_us * GCONFIGF( PLAYBACK_FACTOR ) > timestamp_in_us())
+ if (driver->next_time_us * driver->playback_factor > timestamp_in_us())
return 0;
driver->next_time_us = 0;
@@ -399,7 +401,9 @@ int DriverRegPlayback(SurviveContext *ctx) {
return -1;
}
- SV_INFO("Using playback file '%s' with timefactor of %f", playback_file, GCONFIGF( PLAYBACK_FACTOR ) );
+ survive_attach_configf( ctx, "playback-factor", &sp->playback_factor );
+
+ SV_INFO("Using playback file '%s' with timefactor of %f", playback_file, sp->playback_factor );
SurviveObject *hmd = survive_create_hmd(ctx, "Playback", sp);
SurviveObject *wm0 = survive_create_wm0(ctx, "Playback", sp, 0);
SurviveObject *wm1 = survive_create_wm1(ctx, "Playback", sp, 0);
diff --git a/src/survive_vive.c b/src/survive_vive.c
index c000307..7ee16d0 100755
--- a/src/survive_vive.c
+++ b/src/survive_vive.c
@@ -40,6 +40,7 @@
#endif
#endif
+
struct SurviveViveData;
const short vidpids[] = {
@@ -507,18 +508,22 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *
}
SV_INFO( "Successfully enumerated %s (%d, %d)", devnames[i], did, conf->bNumInterfaces );
+
+ usleep(100000);
}
libusb_free_device_list( devs, 1 );
#endif
+ const char * blacklist = survive_configs(ctx, "blacklist-devs", SC_GET, "-");
+
//Add the drivers - this must happen BEFORE we actually attach interfaces.
- if( sv->udev[USB_DEV_HMD_IMU_LH] ) { survive_add_object( ctx, hmd ); }
- if( sv->udev[USB_DEV_WATCHMAN1] ) { survive_add_object( ctx, wm0 ); }
- if( sv->udev[USB_DEV_WATCHMAN2] ) { survive_add_object( ctx, wm1 ); }
- if( sv->udev[USB_DEV_TRACKER0] ) { survive_add_object( ctx, tr0 ); }
- if( sv->udev[USB_DEV_TRACKER1] ) { survive_add_object( ctx, tr1 ); }
- if( sv->udev[USB_DEV_W_WATCHMAN1] ) { survive_add_object( ctx, ww0 ); }
+ if( !strstr( blacklist, "HMD" ) && sv->udev[USB_DEV_HMD_IMU_LH] ) { survive_add_object( ctx, hmd ); }
+ if( !strstr( blacklist, "WM0" ) && sv->udev[USB_DEV_WATCHMAN1] ) { survive_add_object( ctx, wm0 ); }
+ if( !strstr( blacklist, "WM1" ) && sv->udev[USB_DEV_WATCHMAN2] ) { survive_add_object( ctx, wm1 ); }
+ if( !strstr( blacklist, "TR0" ) && sv->udev[USB_DEV_TRACKER0] ) { survive_add_object( ctx, tr0 ); }
+ if( !strstr( blacklist, "TR1" ) && sv->udev[USB_DEV_TRACKER1] ) { survive_add_object( ctx, tr1 ); }
+ if( !strstr( blacklist, "WW0" ) && sv->udev[USB_DEV_W_WATCHMAN1] ) { survive_add_object( ctx, ww0 ); }
if( sv->udev[USB_DEV_HMD] && AttachInterface( sv, hmd, USB_IF_HMD, sv->udev[USB_DEV_HMD], 0x81, survive_data_cb, "Mainboard" ) ) { return -6; }
if( sv->udev[USB_DEV_HMD_IMU_LH] && AttachInterface( sv, hmd, USB_IF_HMD_IMU_LH, sv->udev[USB_DEV_HMD_IMU_LH], 0x81, survive_data_cb, "Lighthouse" ) ) { return -7; }
@@ -572,7 +577,7 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo
//From actual steam.
if (sv->udev[USB_DEV_HMD])
{
- static uint8_t vive_magic_power_on[64] = { 0x04, 0x78, 0x29, 0x38 };
+ static uint8_t vive_magic_power_on[64] = { 0x04, 0x78, 0x29, 0x38, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x01 };
r = update_feature_report( sv->udev[USB_DEV_HMD], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) );
if( r != sizeof( vive_magic_power_on ) ) return 5;
}
@@ -930,12 +935,6 @@ int survive_get_config( char ** config, SurviveViveData * sv, int devno, int ifa
}
SV_INFO( "Got config data length %d", count );
-
- char fstname[128];
- sprintf( fstname, "calinfo/%d.json.gz", devno );
- FILE * f = fopen( fstname, "wb" );
- fwrite( compressed_data, count, 1, f );
- fclose( f );
int len = survive_simple_inflate( ctx, compressed_data, count, uncompressed_data, sizeof(uncompressed_data)-1 );
if( len <= 0 )
@@ -946,6 +945,13 @@ int survive_get_config( char ** config, SurviveViveData * sv, int devno, int ifa
*config = malloc( len + 1 );
memcpy( *config, uncompressed_data, len );
+
+ char fstname[128];
+ sprintf( fstname, "calinfo/%d.json", devno );
+ FILE * f = fopen( fstname, "wb" );
+ fwrite( uncompressed_data, len, 1, f );
+ fclose( f );
+
return len;
}