aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--Makefile10
-rw-r--r--README.md16
-rw-r--r--include/libsurvive/poser.h9
-rw-r--r--include/libsurvive/survive.h15
-rw-r--r--include/libsurvive/survive_types.h8
-rw-r--r--redist/json_helpers.c28
-rw-r--r--redist/linmath.c12
-rw-r--r--redist/os_generic.c17
-rw-r--r--src/poser_turveytori.c46
-rw-r--r--src/survive_data.c157
-rwxr-xr-xsrc/survive_vive.c10
-rw-r--r--[-rwxr-xr-x]test.c0
12 files changed, 222 insertions, 106 deletions
diff --git a/Makefile b/Makefile
index 1899c4c..54fd6ee 100644
--- a/Makefile
+++ b/Makefile
@@ -25,12 +25,9 @@ GRAPHICS_LOFI:=redist/CNFGFunctions.o redist/CNFGXDriver.o
endif
-POSERS:=src/poser_dummy.o src/poser_daveortho.o src/poser_charlesslow.o
+POSERS:=src/poser_dummy.o src/poser_daveortho.o src/poser_charlesslow.o src/poser_octavioradii.o src/poser_turveytori.o
REDISTS:=redist/json_helpers.o redist/linmath.o redist/jsmn.o redist/os_generic.o
LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_vive.o src/survive_config.o src/survive_cal.o
-LIBSURVIVE_CORE:=$(LIBSURVIVE_CORE)
-LIBSURVIVE_O:=$(POSERS) $(REDISTS) $(LIBSURVIVE_CORE)
-LIBSURVIVE_C:=$(LIBSURVIVE_O:.o=.c)
#If you want to use HIDAPI on Linux.
@@ -48,6 +45,11 @@ LIBSURVIVE_C:=$(LIBSURVIVE_O:.o=.c)
+
+LIBSURVIVE_CORE:=$(LIBSURVIVE_CORE)
+LIBSURVIVE_O:=$(POSERS) $(REDISTS) $(LIBSURVIVE_CORE)
+LIBSURVIVE_C:=$(LIBSURVIVE_O:.o=.c)
+
# unused: redist/crc32.c
test : test.c ./lib/libsurvive.so redist/os_generic.o
diff --git a/README.md b/README.md
index 3b26c5f..130fa1e 100644
--- a/README.md
+++ b/README.md
@@ -26,6 +26,22 @@ Please see the issues for what help needs to be done now!
HackADay article and video with Dr. Yates on how they made the Vive a thing. http://hackaday.com/2016/12/21/alan-yates-why-valves-lighthouse-cant-work/
+
+## Nomenclature
+
+* WRT = With Respect To
+* PoV / POV = Point of View (typically WRT to a LH, sometimes (though rarely) a sensor)
+* LH = Lighthouse = Base Station = A device that produces a 1.8 MHz modulated sync pulse in IR and then sweeps the scene with laser planes.
+* Sync Pulse = A pulse of modulated IR data sent from a ligthhouse, typically by the floodlight aspect of a lighthouse.
+* Sweep Pulse = The evenlope created by a laser sweeping over a light sensor.
+* OOTX = Omnidirectional Optical Transmitter = Data encoded in the sync pulses of the LHs.
+* HMD = Headset = Main sensor receiver with a visual display for a human.
+* WM = Watchman = Controller = The HTC Vive controller.
+* TR = Tracker = Official HTC Tracker.
+* LightcapElement = A single pulse of light, including a timestamp, source sensor and length of pulse.
+* Disambiguator = System that accepts lightcap elements and pulls out OOTX data and relative sweep times of sweep pulses.
+* Poser = Device to convert series of angles from a LH's PoV
+
## Getting things working
There are two things you should consider doing to your system before running libsurvive.
diff --git a/include/libsurvive/poser.h b/include/libsurvive/poser.h
index 497b009..582590e 100644
--- a/include/libsurvive/poser.h
+++ b/include/libsurvive/poser.h
@@ -3,6 +3,11 @@
#include "survive_types.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
typedef enum PoserType_t
{
POSERDATA_NONE = 0,
@@ -55,4 +60,8 @@ typedef struct
typedef int (*PoserCB)( SurviveObject * so, PoserData * pd );
+#ifdef __cplusplus
+};
+#endif
+
#endif
diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h
index 278bbca..e85fe48 100644
--- a/include/libsurvive/survive.h
+++ b/include/libsurvive/survive.h
@@ -5,6 +5,10 @@
#include "survive_types.h"
#include "poser.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
//DANGER: This structure may be redefined. Note that it is logically split into 64-bit chunks
//for optimization on 32- and 64-bit systems.
@@ -38,7 +42,7 @@ struct SurviveObject
//Timing sensitive data (mostly for disambiguation)
int32_t timebase_hz; //48,000,000 for normal vive hardware. (checked)
- int32_t timecenter_ticks; //200,000 for normal vive hardware. (checked)
+ int32_t timecenter_ticks; //200,000 for normal vive hardware. (checked) (This doubles-up as 2x this = full sweep length)
int32_t pulsedist_max_ticks; //500,000 for normal vive hardware. (guessed)
int32_t pulselength_min_sync; //2,200 for normal vive hardware. (guessed)
int32_t pulse_in_clear_time; //35,000 for normal vive hardware. (guessed)
@@ -116,7 +120,7 @@ void survive_install_imu_fn( SurviveContext * ctx, imu_process_func fbp );
void survive_install_angle_fn( SurviveContext * ctx, angle_process_func fbp );
void survive_close( SurviveContext * ctx );
-int survive_poll();
+int survive_poll( SurviveContext * ctx );
SurviveObject * survive_get_so_by_name( SurviveContext * ctx, const char * name );
@@ -128,6 +132,9 @@ int survive_send_magic( SurviveContext * ctx, int magic_code, void * data, int d
//Install the calibrator.
void survive_cal_install( SurviveContext * ctx ); //XXX This will be removed if not already done so.
+// Read back a human-readable string description of the calibration status
+int survive_cal_get_status( struct SurviveContext * ctx, char * description, int description_length );
+
//Call these from your callback if overridden.
//Accept higher-level data.
void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length , uint32_t lh);
@@ -174,5 +181,9 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le );
#define SV_ERROR( ... ) { char stbuff[1024]; sprintf( stbuff, __VA_ARGS__ ); ctx->faultfunction( ctx, stbuff ); }
#define SV_KILL() exit(0) //XXX This should likely be re-defined.
+#ifdef __cplusplus
+};
+#endif
+
#endif
diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h
index bfd0b1d..224719e 100644
--- a/include/libsurvive/survive_types.h
+++ b/include/libsurvive/survive_types.h
@@ -1,6 +1,11 @@
#ifndef _SURVIVE_TYPES_H
#define _SURVIVE_TYPES_H
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
#ifndef FLT
#ifdef USE_DOUBLE
#define FLT double
@@ -39,6 +44,9 @@ typedef int (*DeviceDriver)( SurviveContext * ctx );
typedef int (*DeviceDriverCb)( struct SurviveContext * ctx, void * driver );
typedef int (*DeviceDriverMagicCb)( struct SurviveContext * ctx, void * driver, int magic_code, void * data, int datalen );
+#ifdef __cplusplus
+};
+#endif
#endif
diff --git a/redist/json_helpers.c b/redist/json_helpers.c
index 29d48bd..c52301d 100644
--- a/redist/json_helpers.c
+++ b/redist/json_helpers.c
@@ -50,44 +50,46 @@ void json_write_float_array(FILE* f, const char* tag, float* v, uint8_t count) {
uint8_t i = 0;
char * str1 = NULL;
char * str2 = NULL;
- asprintf(&str1,"\"%s\":[", tag);
+ if( asprintf(&str1,"\"%s\":[", tag) < 0 ) goto giveup;
for (i=0;i<count;++i) {
if ( (i+1) < count) {
- asprintf(&str2, "%s\"%f\"", str1,v[i]);
+ if( asprintf(&str2, "%s\"%f\"", str1,v[i]) < 0 ) goto giveup;
} else {
- asprintf(&str2, "%s\"%f\",", str1,v[i]);
+ if( asprintf(&str2, "%s\"%f\",", str1,v[i]) < 0 ) goto giveup;
}
free(str1);
str1=str2;
str2=NULL;
}
- asprintf(&str2, "%s]", str1);
+ if( asprintf(&str2, "%s]", str1) < 0 ) goto giveup;
fputs(str2,f);
- free(str1);
- free(str2);
+giveup:
+ if( str1 ) free(str1);
+ if( str2 ) free(str2);
}
void json_write_double_array(FILE* f, const char* tag, double* v, uint8_t count) {
uint8_t i = 0;
char * str1 = NULL;
char * str2 = NULL;
- asprintf(&str1,"\"%s\":[", tag);
+ if( asprintf(&str1,"\"%s\":[", tag) < 0 ) goto giveup;
for (i=0;i<count;++i) {
if (i<(count-1)) {
- asprintf(&str2, "%s\"%f\",", str1,v[i]);
+ if( asprintf(&str2, "%s\"%f\",", str1,v[i]) < 0 ) goto giveup;
} else {
- asprintf(&str2, "%s\"%f\"", str1,v[i]);
+ if( asprintf(&str2, "%s\"%f\"", str1,v[i]) < 0 ) goto giveup;
}
free(str1);
str1=str2;
str2=NULL;
}
- asprintf(&str2, "%s]", str1);
+ if( asprintf(&str2, "%s]", str1) < 0 ) goto giveup;
fputs(str2,f);
- free(str1);
- free(str2);
+giveup:
+ if( str1 ) free(str1);
+ if( str2 ) free(str2);
}
void json_write_uint32(FILE* f, const char* tag, uint32_t v) {
@@ -119,7 +121,7 @@ char* load_file_to_mem(const char* path) {
fseek( f, 0, SEEK_SET );
char * JSON_STRING = malloc( len + 1);
memset(JSON_STRING,0,len+1);
- fread( JSON_STRING, len, 1, f );
+ int i = fread( JSON_STRING, len, 1, f ); i = i; //Ignore return value.
fclose( f );
return JSON_STRING;
}
diff --git a/redist/linmath.c b/redist/linmath.c
index caff1de..5fefe1e 100644
--- a/redist/linmath.c
+++ b/redist/linmath.c
@@ -520,17 +520,15 @@ void quatfrom2vectors(FLT *q, const FLT *src, const FLT *dest)
FLT invs = 1 / s;
FLT c[3];
- //cross3d(c, v0, v1);
- cross3d(c, v1, v0);
+ cross3d(c, v0, v1);
- q[0] = c[0] * invs;
- q[1] = c[1] * invs;
- q[2] = c[2] * invs;
- q[3] = s * 0.5f;
+ q[0] = s * 0.5f;
+ q[1] = c[0] * invs;
+ q[2] = c[1] * invs;
+ q[3] = c[2] * invs;
quatnormalize(q, q);
}
-
}
void matrix44copy(FLT * mout, const FLT * minm )
diff --git a/redist/os_generic.c b/redist/os_generic.c
index 1ab4863..3191357 100644
--- a/redist/os_generic.c
+++ b/redist/os_generic.c
@@ -151,8 +151,9 @@ void OGDeleteSema( og_sema_t os )
#else
-#define _GNU_SOURCE
-
+#ifndef _GNU_SOURCE
+# define _GNU_SOURCE
+#endif
#include <sys/stat.h>
#include <stdlib.h>
@@ -198,7 +199,7 @@ double OGGetFileTime( const char * file )
og_thread_t OGCreateThread( void * (routine)( void * ), void * parameter )
{
- pthread_t * ret = malloc( sizeof( pthread_t ) );
+ pthread_t * ret = (pthread_t *)malloc( sizeof( pthread_t ) );
int r = pthread_create( ret, 0, routine, parameter );
if( r )
{
@@ -277,7 +278,7 @@ void OGDeleteMutex( og_mutex_t om )
og_sema_t OGCreateSema()
{
- sem_t * sem = malloc( sizeof( sem_t ) );
+ sem_t * sem = (sem_t *)malloc( sizeof( sem_t ) );
sem_init( sem, 0, 0 );
return (og_sema_t)sem;
}
@@ -285,24 +286,24 @@ og_sema_t OGCreateSema()
int OGGetSema( og_sema_t os )
{
int valp;
- sem_getvalue( os, &valp );
+ sem_getvalue( (sem_t *)os, &valp );
return valp;
}
void OGLockSema( og_sema_t os )
{
- sem_wait( os );
+ sem_wait( (sem_t *)os );
}
void OGUnlockSema( og_sema_t os )
{
- sem_post( os );
+ sem_post( (sem_t *)os );
}
void OGDeleteSema( og_sema_t os )
{
- sem_destroy( os );
+ sem_destroy( (sem_t *)os );
free(os);
}
diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c
index c251040..7abf5d0 100644
--- a/src/poser_turveytori.c
+++ b/src/poser_turveytori.c
@@ -83,6 +83,9 @@ typedef struct
FLT oldAngles[SENSORS_PER_OBJECT][2][NUM_LIGHTHOUSES][OLD_ANGLES_BUFF_LEN]; // sensor, sweep axis, lighthouse, instance
int angleIndex[NUM_LIGHTHOUSES][2]; // index into circular buffer ahead. separate index for each axis.
int lastAxis[NUM_LIGHTHOUSES];
+
+ Point lastLhPos[NUM_LIGHTHOUSES];
+ FLT lastLhRotAxisAngle[NUM_LIGHTHOUSES][4];
} ToriData;
@@ -433,23 +436,25 @@ Point getGradient(Point pointIn, PointsAndAngle *pna, size_t pnaCount, FLT preci
{
Point result;
+ FLT baseFitness = getPointFitness(pointIn, pna, pnaCount, 0);
+
Point tmpXplus = pointIn;
Point tmpXminus = pointIn;
tmpXplus.x = pointIn.x + precision;
tmpXminus.x = pointIn.x - precision;
- result.x = getPointFitness(tmpXplus, pna, pnaCount, 0) - getPointFitness(tmpXminus, pna, pnaCount, 0);
+ result.x = baseFitness - getPointFitness(tmpXminus, pna, pnaCount, 0);
Point tmpYplus = pointIn;
Point tmpYminus = pointIn;
tmpYplus.y = pointIn.y + precision;
tmpYminus.y = pointIn.y - precision;
- result.y = getPointFitness(tmpYplus, pna, pnaCount, 0) - getPointFitness(tmpYminus, pna, pnaCount, 0);
+ result.y = baseFitness - getPointFitness(tmpYminus, pna, pnaCount, 0);
Point tmpZplus = pointIn;
Point tmpZminus = pointIn;
tmpZplus.z = pointIn.z + precision;
tmpZminus.z = pointIn.z - precision;
- result.z = getPointFitness(tmpZplus, pna, pnaCount, 0) - getPointFitness(tmpZminus, pna, pnaCount, 0);
+ result.z = baseFitness - getPointFitness(tmpZminus, pna, pnaCount, 0);
return result;
}
@@ -1144,6 +1149,8 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh)
static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *obj, SurviveObject *so, char doLogOutput, int lh, int setLhCalibration)
{
+ ToriData *toriData = so->PoserData;
+
//printf("Solving for Lighthouse\n");
//printf("obj->numSensors = %d;\n", obj->numSensors);
@@ -1234,6 +1241,14 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
// back into the search for the correct point (see "if (a1 > M_PI / 2)" below)
Point p1 = getNormalizedAndScaledVector(avgNorm, 8);
+ // if the last lighthouse position has been populated (extremely rare it would be 0)
+ if (toriData->lastLhPos[lh].x != 0)
+ {
+ p1.x = toriData->lastLhPos[lh].x;
+ p1.y = toriData->lastLhPos[lh].y;
+ p1.z = toriData->lastLhPos[lh].z;
+ }
+
Point refinedEstimateGd = RefineEstimateUsingModifiedGradientDescent1(p1, pna, pnaCount, logFile);
FLT pf1[3] = { refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z };
@@ -1258,11 +1273,29 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
//printf("Distance is %f, Fitness is %f\n", distance, fitGd);
FLT rot[4]; // this is axis/ angle rotation, not a quaternion!
+
+ if (toriData->lastLhRotAxisAngle[lh][0] != 0)
+ {
+ rot[0] = toriData->lastLhRotAxisAngle[lh][0];
+ rot[1] = toriData->lastLhRotAxisAngle[lh][1];
+ rot[2] = toriData->lastLhRotAxisAngle[lh][2];
+ rot[3] = toriData->lastLhRotAxisAngle[lh][3];
+ }
+
+
SolveForRotation(rot, obj, refinedEstimateGd);
FLT objPos[3];
+ {
+ toriData->lastLhRotAxisAngle[lh][0] = rot[0];
+ toriData->lastLhRotAxisAngle[lh][1] = rot[1];
+ toriData->lastLhRotAxisAngle[lh][2] = rot[2];
+ toriData->lastLhRotAxisAngle[lh][3] = rot[3];
+ }
+
WhereIsTheTrackedObjectAxisAngle(objPos, rot, refinedEstimateGd);
+
FLT rotQuat[4];
quatfromaxisangle(rotQuat, rot, rot[3]);
@@ -1325,6 +1358,11 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
fclose(logFile);
}
+
+ toriData->lastLhPos[lh].x = refinedEstimateGd.x;
+ toriData->lastLhPos[lh].y = refinedEstimateGd.y;
+ toriData->lastLhPos[lh].z = refinedEstimateGd.z;
+
return refinedEstimateGd;
}
@@ -1482,7 +1520,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
counter++;
// let's just do this occasionally for now...
- if (counter % 2 == 0)
+ if (counter % 4 == 0)
QuickPose(so, 0);
}
// axis changed, time to increment the circular buffer index.
diff --git a/src/survive_data.c b/src/survive_data.c
index 22ce8c2..df8df8e 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -4,9 +4,9 @@
#include "survive_internal.h"
#include <stdint.h>
#include <string.h>
-#include <math.h>
+#include <math.h> /* for sqrt */
-#define USE_TURVEYBIGUATOR
+//#define USE_TURVEYBIGUATOR
#ifdef USE_TURVEYBIGUATOR
@@ -76,6 +76,7 @@ int handle_lightcap2_getAcodeFromSyncPulse(SurviveObject * so, int pulseLen)
}
uint8_t remove_outliers(SurviveObject *so) {
+ return 0; // disabling this for now because it seems remove almost all the points for wired watchman and wired tracker.
lightcap2_data *lcd = so->disambiguator_data;
uint32_t sum = 0;
@@ -470,13 +471,59 @@ void handle_lightcap2( SurviveObject * so, LightcapElement * le )
int32_t decode_acode(uint32_t length, int32_t main_divisor) {
//+50 adds a small offset and seems to help always get it right.
//Check the +50 in the future to see how well this works on a variety of hardware.
-
+ if( !main_divisor ) return -1;
int32_t acode = (length+main_divisor+50)/(main_divisor*2);
if( acode & 1 ) return -1;
return (acode>>1) - 6;
}
+
+void HandleOOTX( SurviveContext * ctx, SurviveObject * so )
+{
+ int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz.
+
+ int32_t acode_array[2] =
+ {
+ decode_acode(so->last_sync_length[0],main_divisor),
+ decode_acode(so->last_sync_length[1],main_divisor)
+ };
+
+
+ int32_t delta1 = so->last_sync_time[0] - so->recent_sync_time;
+ int32_t delta2 = so->last_sync_time[1] - so->last_sync_time[0];
+
+ //printf( "%p %p %d %d %d %p\n", ctx, so, so->last_sync_time[0], acode_array, so->last_sync_length[0], ctx->lightproc );
+ if( acode_array[0] >= 0 ) ctx->lightproc( so, -1, acode_array[0], delta1, so->last_sync_time[0], so->last_sync_length[0], 0 );
+ if( acode_array[1] >= 0 ) ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1], 1 );
+
+ so->recent_sync_time = so->last_sync_time[1];
+
+/*
+ //Throw out everything if our sync pulses look like they're bad.
+ //This actually doesn't seem to hold anymore, now that we're looking for multiple LHs.
+ int32_t center_1 = so->timecenter_ticks*2 - so->pulse_synctime_offset;
+ int32_t center_2 = so->pulse_synctime_offset;
+ int32_t slack = so->pulse_synctime_slack;
+
+ if( delta1 < center_1 - slack || delta1 > center_1 + slack )
+ {
+ //XXX: TODO: Count faults.
+ so->sync_set_number = -1;
+ return;
+ }
+
+ if( delta2 < center_2 - slack || delta2 > center_2 + slack )
+ {
+ //XXX: TODO: Count faults.
+ so->sync_set_number = -1;
+ return;
+ }
+*/
+ so->did_handle_ootx = 1;
+}
+
+
//This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode.
void handle_lightcap( SurviveObject * so, LightcapElement * le )
{
@@ -487,6 +534,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
return;
#else
+ //printf( "LE%3d%6d%12d\n", le->sensor_id, le->length, le->timestamp );
//int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time;
@@ -520,6 +568,9 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
//unified driver.
int ssn = so->sync_set_number; //lighthouse number
if( ssn < 0 ) ssn = 0;
+#ifdef DEBUG
+ if( ssn >= NUM_LIGHTHOUSES ) { SV_INFO( "ALGORITHMIC WARNING: ssn exceeds NUM_LIGHTHOUSES" ); }
+#endif
int last_sync_time = so->last_sync_time [ssn];
int last_sync_length = so->last_sync_length[ssn];
int32_t delta = le->timestamp - last_sync_time; //Handle time wrapping (be sure to be int32)
@@ -537,15 +588,37 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
{
int is_new_pulse = delta > so->pulselength_min_sync /*1500*/ + last_sync_length;
+ //TRICKY: If we didn't see anything from the other lighthouse, we might just not see it... But, we still have to send our sync
+ //information to the rest of libsurvive. This could be turned into a function and combined with the code below.
+ if( !so->did_handle_ootx && is_new_pulse )
+ {
+ HandleOOTX( ctx, so );
+ }
so->did_handle_ootx = 0;
+
+ //printf( "INP: %d %d\n", is_new_pulse, so->sync_set_number );
+
if( is_new_pulse )
{
int is_master_sync_pulse = delta > so->pulse_in_clear_time /*40000*/;
+ int is_pulse_from_same_lh_as_last_sweep;
+ int tp = delta % ( so->timecenter_ticks * 2);
+ is_pulse_from_same_lh_as_last_sweep = tp < so->pulse_synctime_slack && tp > -so->pulse_synctime_slack;
- if( is_master_sync_pulse )
+ if( is_master_sync_pulse ) //Could also be called by slave if no master was seen.
{
- ssn = so->sync_set_number = 0;
+ ssn = so->sync_set_number = is_pulse_from_same_lh_as_last_sweep?(so->sync_set_number):0; //If repeated lighthouse, just back off one.
+ if( ssn < 0 ) { SV_INFO( "SEVERE WARNING: Pulse codes for tracking not able to be backed out.\n" ); ssn = 0; }
+ if( ssn != 0 )
+ {
+ //If it's the slave that is repeated, be sure to zero out its sync info.
+ so->last_sync_length[0] = 0;
+ }
+ else
+ {
+ so->last_sync_length[1] = 0;
+ }
so->last_sync_time[ssn] = le->timestamp;
so->last_sync_length[ssn] = le->length;
}
@@ -582,15 +655,20 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
}
}
- //Extra tidbit for storing length-of-sync-pulses.
+#if 0
+ //Extra tidbit for storing length-of-sync-pulses, if you want to try to use this to determine AoI or distance to LH.
+ //We don't actually use this anywhere, and I doubt we ever will? Though, it could be useful at a later time to improve tracking.
{
int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz.
int base_station = is_new_pulse;
- //printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length );
- ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length, base_station);
+ printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length ); //XXX sync_set_number is wrong here.
+ ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length, base_station); //XXX sync_set_number is wrong here.
}
+#endif
}
+ //Any else- statements below here are
+
//See if this is a valid actual pulse.
else if( le->length < so->pulse_max_for_sweep && delta > so->pulse_in_clear_time && ssn >= 0 )
{
@@ -607,68 +685,20 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
//Make sure it fits nicely into a divisible-by-500 time.
int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz.
-
- int32_t acode_array[2] =
- {
- decode_acode(so->last_sync_length[0],main_divisor),
- decode_acode(so->last_sync_length[1],main_divisor)
- };
-
- //XXX: TODO: Capture error count here.
- if( acode_array[0] < 0 ) return;
- if( acode_array[1] < 0 ) return;
-
- int acode = acode_array[0];
+ int acode = decode_acode(so->last_sync_length[0],main_divisor);
if( !so->did_handle_ootx )
- {
- int32_t delta1 = so->last_sync_time[0] - so->recent_sync_time;
- int32_t delta2 = so->last_sync_time[1] - so->last_sync_time[0];
-
- ctx->lightproc( so, -1, acode_array[0], delta1, so->last_sync_time[0], so->last_sync_length[0], 0 );
- ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1], 1 );
-
- so->recent_sync_time = so->last_sync_time[1];
-
- //Throw out everything if our sync pulses look like they're bad.
-
- int32_t center_1 = so->timecenter_ticks*2 - so->pulse_synctime_offset;
- int32_t center_2 = so->pulse_synctime_offset;
- int32_t slack = so->pulse_synctime_slack;
-
- if( delta1 < center_1 - slack || delta1 > center_1 + slack )
- {
- //XXX: TODO: Count faults.
- so->sync_set_number = -1;
- return;
- }
-
- if( delta2 < center_2 - slack || delta2 > center_2 + slack )
- {
- //XXX: TODO: Count faults.
- so->sync_set_number = -1;
- return;
- }
-
- so->did_handle_ootx = 1;
- }
-
- if (acode > 3) {
- if( ssn == 0 )
- {
- //SV_INFO( "Warning: got a slave marker but only got a master sync." );
- //This happens too frequently. Consider further examination.
- }
- dl = so->last_sync_time[1];
- tpco = so->last_sync_length[1];
- }
+ HandleOOTX( ctx, so );
int32_t offset_from = le->timestamp - dl + le->length/2;
//Make sure pulse is in valid window
- if( offset_from < 380000 && offset_from > 70000 )
+ if( offset_from < so->timecenter_ticks*2-so->pulse_in_clear_time && offset_from > so->pulse_in_clear_time )
{
- ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length, !(acode>>2) );
+ int whichlh;
+ if( acode < 0 ) whichlh = 1;
+ else whichlh = !(acode>>2);
+ ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length, whichlh );
}
}
else
@@ -677,7 +707,6 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
//Runt pulse, or no sync pulses available.
}
#endif
-
}
diff --git a/src/survive_vive.c b/src/survive_vive.c
index 8e22ea1..030db8a 100755
--- a/src/survive_vive.c
+++ b/src/survive_vive.c
@@ -54,7 +54,7 @@ const short vidpids[] = {
const char * devnames[] = {
"HMD",
- "Lighthouse",
+ "HMD IMU & LH",
"Watchman 1",
"Watchman 2",
"Tracker 0",
@@ -344,7 +344,8 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *
if (cur_dev->vendor_id == vendor_id &&
cur_dev->product_id == product_id)
{
- if( cur_dev->interface_number == enumid )
+ if( cur_dev->interface_number == enumid ||
+ cur_dev->interface_number == -1 && menum == enumid)
{
path_to_open = cur_dev->path;
break;
@@ -370,6 +371,7 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *
wchar_t wstr[255];
res = hid_get_serial_number_string(handle, wstr, 255);
+ printf("Found %s. ", devnames[i]);
wprintf(L"Serial Number String: (%d) %s for %04x:%04x@%d (Dev: %p)\n", wstr[0], wstr,vendor_id, product_id, menum, handle);
sv->udev[i] = handle;
@@ -1164,7 +1166,7 @@ void survive_data_cb( SurviveUSBInterface * si )
le.sensor_id = POP1;
le.length = POP2;
le.timestamp = POP4;
- if( le.sensor_id == 0xff ) break;
+ if( le.sensor_id > 0xfd ) continue;
handle_lightcap( obj, &le );
}
break;
@@ -1182,7 +1184,7 @@ void survive_data_cb( SurviveUSBInterface * si )
le.sensor_id = (uint8_t)POP2;
le.length = POP2;
le.timestamp = POP4;
- if( le.sensor_id == 0xff ) break;
+ if( le.sensor_id > 0xfd ) continue; //
handle_lightcap( obj, &le );
}
break;
diff --git a/test.c b/test.c
index 4909d50..4909d50 100755..100644
--- a/test.c
+++ b/test.c