diff options
-rw-r--r-- | Makefile | 15 | ||||
-rw-r--r-- | calibrate.c | 9 | ||||
-rw-r--r-- | calibrate_client.c | 9 | ||||
-rw-r--r-- | data_recorder.c | 6 | ||||
-rw-r--r-- | include/libsurvive/poser.h | 26 | ||||
-rw-r--r-- | include/libsurvive/survive.h | 115 | ||||
-rw-r--r-- | include/libsurvive/survive_types.h | 27 | ||||
-rw-r--r-- | redist/linmath.c | 7 | ||||
-rw-r--r-- | src/poser_dummy.c | 52 | ||||
-rw-r--r-- | src/survive.c | 58 | ||||
-rw-r--r-- | src/survive_cal.c | 6 | ||||
-rw-r--r-- | src/survive_cal.h | 16 | ||||
-rw-r--r-- | src/survive_cal_lhfind.c | 14 | ||||
-rw-r--r-- | src/survive_config.c | 5 | ||||
-rw-r--r-- | src/survive_config.h | 4 | ||||
-rw-r--r-- | src/survive_data.c | 4 | ||||
-rw-r--r-- | src/survive_driverman.c | 2 | ||||
-rw-r--r-- | src/survive_driverman.h | 38 | ||||
-rw-r--r-- | src/survive_internal.h | 89 | ||||
-rw-r--r-- | src/survive_process.c | 39 | ||||
-rw-r--r-- | src/survive_vive.c | 98 |
21 files changed, 363 insertions, 276 deletions
@@ -3,27 +3,32 @@ all : lib data_recorder test calibrate calibrate_client CFLAGS:=-Iinclude/libsurvive -I. -fPIC -g -O0 -Iredist -flto -DUSE_DOUBLE -std=gnu99 LDFLAGS:=-lpthread -lusb-1.0 -lz -lX11 -lm -flto -g - CALS:=src/survive_cal_lhfind.o src/survive_cal.o +POSERS:=src/poser_dummy.o +REDISTS:=redist/json_helpers.o redist/linmath.o redist/jsmn.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 +LIBSURVIVE_O:=$(CALS) $(POSERS) $(REDISTS) $(LIBSURVIVE_CORE) + +GRAPHICS_LOFI:=redist/DrawFunctions.o redist/XDriver.o # unused: redist/crc32.c test : test.c lib/libsurvive.so redist/os_generic.o gcc -o $@ $^ $(LDFLAGS) $(CFLAGS) -data_recorder : data_recorder.c lib/libsurvive.so redist/os_generic.o redist/DrawFunctions.o redist/XDriver.o +data_recorder : data_recorder.c lib/libsurvive.so redist/os_generic.c $(GRAPHICS_LOFI) gcc -o $@ $^ $(LDFLAGS) $(CFLAGS) -calibrate : calibrate.c lib/libsurvive.so redist/os_generic.c redist/DrawFunctions.c redist/XDriver.c +calibrate : calibrate.c lib/libsurvive.so redist/os_generic.c $(GRAPHICS_LOFI) gcc -o $@ $^ $(LDFLAGS) $(CFLAGS) -calibrate_client : calibrate_client.c lib/libsurvive.so redist/os_generic.c redist/DrawFunctions.c redist/XDriver.c +calibrate_client : calibrate_client.c lib/libsurvive.so redist/os_generic.c $(GRAPHICS_LOFI) gcc -o $@ $^ $(LDFLAGS) $(CFLAGS) lib: mkdir lib -lib/libsurvive.so : src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o redist/jsmn.o src/ootx_decoder.o redist/linmath.o src/survive_driverman.o src/survive_vive.o src/survive_config.o redist/json_helpers.o $(DEBUGSTUFF) $(CALS) +lib/libsurvive.so : $(LIBSURVIVE_O) gcc -o $@ $^ $(LDFLAGS) -shared clean : diff --git a/calibrate.c b/calibrate.c index a04c269..c557251 100644 --- a/calibrate.c +++ b/calibrate.c @@ -76,15 +76,14 @@ void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int } } -void my_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ) +void my_imu_process( struct SurviveObject * so, int mask, FLT * accelgyro, uint32_t timecode, int id ) { - survive_default_imu_process( so, accelgyro, timecode, id ); + survive_default_imu_process( so, mask, accelgyro, timecode, id ); -return; //if( so->codename[0] == 'H' ) - if( 1 ) + if( 0 ) { - printf( "I %s %d %d %d %d %d %d %d %d\n", so->codename, timecode, accelgyro[0], accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id ); + printf( "I %s %d %f %f %f %f %f %f %d\n", so->codename, timecode, accelgyro[0], accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id ); } } diff --git a/calibrate_client.c b/calibrate_client.c index 9867d67..b15b9db 100644 --- a/calibrate_client.c +++ b/calibrate_client.c @@ -75,15 +75,14 @@ void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int } } -void my_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ) +void my_imu_process( struct SurviveObject * so, int mask, FLT * accelgyro, uint32_t timecode, int id ) { - survive_default_imu_process( so, accelgyro, timecode, id ); + survive_default_imu_process( so, mask, accelgyro, timecode, id ); -return; //if( so->codename[0] == 'H' ) - if( 1 ) + if( 0 ) { - printf( "I %s %d %d %d %d %d %d %d %d\n", so->codename, timecode, accelgyro[0], accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id ); + printf( "I %s %d %f %f %f %f %f %f %d\n", so->codename, timecode, accelgyro[0], accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id ); } } diff --git a/data_recorder.c b/data_recorder.c index 951f234..5504d42 100644 --- a/data_recorder.c +++ b/data_recorder.c @@ -77,15 +77,15 @@ void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int } -void my_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ) +void my_imu_process( struct SurviveObject * so, int mask, FLT * accelgyro, uint32_t timecode, int id ) { - survive_default_imu_process( so, accelgyro, timecode, id ); + survive_default_imu_process( so, mask, accelgyro, timecode, id ); //return; //if( so->codename[0] == 'H' ) if( 1 ) { - printf( "I %s %d %d %d %d %d %d %d %d\n", so->codename, timecode, accelgyro[0], accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id ); + printf( "I %s %d %f %f %f %f %f %f %d\n", so->codename, timecode, accelgyro[0], accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id ); } } diff --git a/include/libsurvive/poser.h b/include/libsurvive/poser.h index 4894acf..98c926e 100644 --- a/include/libsurvive/poser.h +++ b/include/libsurvive/poser.h @@ -9,24 +9,26 @@ typedef enum PoserType_t POSERDATA_IMU, POSERDATA_LIGHT, //Single lighting event. POSERDATA_FULL_SCENE, //Full, statified X, Y sweep data for both lighthouses. + POSERDATA_DISASSOCIATE, //If you get this, it doesn't contain data. It just tells you to please disassociate from the current SurviveObject and delete your poserdata. } PoserType; -struct PoserData +typedef struct { PoserType pt; uint8_t data[0]; -}; +} PoserData; -struct PoserDataIMU +typedef struct { PoserType pt; uint8_t datamask; //0 = accel present, 1 = gyro present, 2 = mag present. FLT accel[3]; FLT gyro[3]; FLT mag[3]; -}; + uint32_t timecode; //In object-local ticks. +} PoserDataIMU; -struct PoserDataLight +typedef struct { PoserType pt; int sensor_id; @@ -34,21 +36,21 @@ struct PoserDataLight uint32_t timecode; //In object-local ticks. FLT length; //In seconds FLT angle; //In radians from center of lighthouse. -}; +} PoserDataLight; -struct PoserDataFullScene +typedef struct { PoserType pt; //If "lengths[...]" < 0, means not a valid piece of sweep information. FLT lengths[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; - FLT angles [SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; //2 Axes + FLT angles [SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; //2 Axes (Angles in LH space) - struct PoserDataIMU lastimu; -}; + PoserDataIMU lastimu; +} PoserDataFullScene; -//When you register your posers using the internal system, -typedef int (*PoserCB)( struct SurviveObject * so, struct PoserData * pd ); +//When you write your posers, use the following definition, and register with REGISTER_LINKTIME. +typedef int (*PoserCB)( SurviveObject * so, PoserData * pd ); #endif diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 536c7fb..7fd6046 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -2,16 +2,15 @@ #define _SURVIVE_H #include <stdint.h> +#include "survive_types.h" #include "poser.h" -struct SurviveContext; - //DANGER: This structure may be redefined. Note that it is logically split into 64-bit chunks //for optimization on 32- and 64-bit systems. struct SurviveObject { - struct SurviveContext * ctx; + SurviveContext * ctx; char codename[4]; //3 letters, null-terminated. Currently HMD, WM0, WM1. char drivername[4]; //3 letters for driver. Currently "HTC" @@ -25,12 +24,12 @@ struct SurviveObject int8_t ison:1; int8_t additional_flags:6; - //Pose Information, also "resolver" field. + //Pose Information, also "poser" field. FLT PoseConfidence; //0..1 - FLT Position[3]; - FLT Rotation[4]; + SurvivePose OutPose; + SurvivePose FromLHPose[NUM_LIGHTHOUSES]; //Optionally filled out by poser, contains computed position from each lighthouse. void * PoserData; //Initialized to zero, configured by poser, can be anything the poser wants. - PoserCB * PoserFn; + PoserCB PoserFn; //Device-specific information about the location of the sensors. This data will be used by the poser. int8_t nr_locations; @@ -62,34 +61,106 @@ struct SurviveObject int tsl; }; -struct SurviveContext * survive_init( int headless ); -//For any of these, you may pass in 0 for the function pointer to use default behavior. -void survive_install_info_fn( struct SurviveContext * ctx, text_feedback_func fbp ); -void survive_install_error_fn( struct SurviveContext * ctx, text_feedback_func fbp ); -void survive_install_light_fn( struct SurviveContext * ctx, light_process_func fbp ); -void survive_install_imu_fn( struct SurviveContext * ctx, imu_process_func fbp ); -void survive_install_angle_fn( struct SurviveContext * ctx, angle_process_func fbp ); +struct BaseStationData +{ + uint8_t PositionSet:1; + + SurvivePose Pose; + + uint8_t OOTXSet:1; + uint32_t BaseStationID; + FLT fcalphase[2]; + FLT fcaltilt[2]; + FLT fcalcurve[2]; + FLT fcalgibpha[2]; + FLT fcalgibmag[2]; +}; + +struct SurviveContext +{ + text_feedback_func faultfunction; + text_feedback_func notefunction; + light_process_func lightproc; + imu_process_func imuproc; + angle_process_func angleproc; + + //Calibration data: + BaseStationData bsd[NUM_LIGHTHOUSES]; + + SurviveCalData * calptr; //If and only if the calibration subsystem is attached. + + SurviveObject ** objs; + int objs_ct; + + void ** drivers; + DeviceDriverCb * driverpolls; + DeviceDriverCb * drivercloses; + DeviceDriverMagicCb * drivermagics; + int driver_ct; +}; + +SurviveContext * survive_init( int headless ); -void survive_close( struct SurviveContext * ctx ); +//For any of these, you may pass in 0 for the function pointer to use default behavior. +//In general unless you are doing wacky things like recording or playing back data, you won't need to use this. +void survive_install_info_fn( SurviveContext * ctx, text_feedback_func fbp ); +void survive_install_error_fn( SurviveContext * ctx, text_feedback_func fbp ); +void survive_install_light_fn( SurviveContext * ctx, light_process_func fbp ); +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(); -struct SurviveObject * survive_get_so_by_name( struct SurviveContext * ctx, const char * name ); +SurviveObject * survive_get_so_by_name( SurviveContext * ctx, const char * name ); //Utilitiy functions. -int survive_simple_inflate( struct SurviveContext * ctx, const char * input, int inlen, char * output, int outlen ); +int survive_simple_inflate( SurviveContext * ctx, const char * input, int inlen, char * output, int outlen ); -int survive_send_magic( struct SurviveContext * ctx, int magic_code, void * data, int datalen ); +int survive_send_magic( SurviveContext * ctx, int magic_code, void * data, int datalen ); //Install the calibrator. -void survive_cal_install( struct SurviveContext * ctx ); +void survive_cal_install( SurviveContext * ctx ); //XXX This will be removed if not already done so. //Call these from your callback if overridden. //Accept higher-level data. -void survive_default_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); -void survive_default_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ); -void survive_default_angle_process( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); +void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); +void survive_default_imu_process( SurviveObject * so, int mode, FLT * accelgyro, uint32_t timecode, int id ); +void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); + + +////////////////////// Survive Drivers //////////////////////////// + +void RegisterDriver( const char * name, void * data ); +#define REGISTER_LINKTIME( func ) \ + void __attribute__((constructor)) Register##func() { RegisterDriver( #func, &func ); } + + + +///////////////////////// General stuff for writing drivers /////// + +//For device drivers to call. This actually attaches them. +int survive_add_object( SurviveContext * ctx, SurviveObject * obj ); +void survive_add_driver( SurviveContext * ctx, void * payload, DeviceDriverCb poll, DeviceDriverCb close, DeviceDriverMagicCb magic ); + +//For lightcap, etc. Don't change this structure at all. Regular vive is dependent on it being exactly as-is. +//When you write drivers, you can use this to send survive lightcap data. +typedef struct +{ + uint8_t sensor_id; + uint8_t type; //Mostly unused. Set to 255 to ignore it. + uint16_t length; + uint32_t timestamp; +} __attribute__((packed)) LightcapElement; + +//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 ); + +#define SV_INFO( x... ) { char stbuff[1024]; sprintf( stbuff, x ); ctx->notefunction( ctx, stbuff ); } +#define SV_ERROR( x... ) { char stbuff[1024]; sprintf( stbuff, x ); ctx->faultfunction( ctx, stbuff ); } +#define SV_KILL() exit(0) //XXX This should likely be re-defined. #endif diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h index 593819f..1600e11 100644 --- a/include/libsurvive/survive_types.h +++ b/include/libsurvive/survive_types.h @@ -9,6 +9,11 @@ #endif #endif +typedef struct SurvivePose +{ + FLT Pos[3]; + FLT Rot[4]; +} SurvivePose; //Careful with this, you can't just add another one right now, would take minor changes in survive_data.c and the cal tools. //It will also require a recompile. TODO: revisit this and correct the comment once fixed. @@ -17,13 +22,23 @@ #define INTBUFFSIZE 64 #define SENSORS_PER_OBJECT 32 -struct SurviveObject; -struct SurviveContext; +typedef struct SurviveObject SurviveObject; +typedef struct SurviveContext SurviveContext; +typedef struct BaseStationData BaseStationData; +typedef struct SurviveCalData SurviveCalData; //XXX Warning: This may be removed. Check at a later time for its defunctness. + +typedef void (*text_feedback_func)( SurviveContext * ctx, const char * fault ); +typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); +typedef void (*imu_process_func)( SurviveObject * so, int mask, FLT * accelgyro, uint32_t timecode, int id ); +typedef void (*angle_process_func)( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); + + +//Device drivers (prefix your drivers with "DriverReg") i.e. +// REGISTER_LINKTIME( DriverRegHTCVive ); +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 ); -typedef void (*text_feedback_func)( struct SurviveContext * ctx, const char * fault ); -typedef void (*light_process_func)( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); -typedef void (*imu_process_func)( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ); -typedef void (*angle_process_func)( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); #endif diff --git a/redist/linmath.c b/redist/linmath.c index 157141f..1c5c25b 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -93,11 +93,6 @@ void quatsetnone(FLT * q) q[0] = 1; q[1] = 0; q[2] = 0; q[3] = 0; } -void quatsetidentity(FLT * q) -{ - q[0] = 1; q[1] = 0; q[2] = 0; q[3] = 1; -} - void quatcopy(FLT * qout, const FLT * qin) { qout[0] = qin[0]; @@ -447,7 +442,7 @@ void quatfrom2vectors(FLT *q, const FLT *src, const FLT *dest) // If dot == 1, vectors are the same if (d >= 1.0f) { - quatsetidentity(q); + quatsetnone(q); return; } if (d < (1e-6f - 1.0f)) diff --git a/src/poser_dummy.c b/src/poser_dummy.c new file mode 100644 index 0000000..67f8edb --- /dev/null +++ b/src/poser_dummy.c @@ -0,0 +1,52 @@ +#include <survive.h> +#include <stdio.h> +#include <stdlib.h> + +typedef struct +{ + int something; + //Stuff +} DummyData; + +int PoserDummy( SurviveObject * so, PoserData * pd ) +{ + PoserType pt = pd->pt; + SurviveContext * ctx = so->ctx; + DummyData * dd = so->PoserData; + + if( !dd ) so->PoserData = dd = malloc( sizeof( DummyData ) ); + + switch( pt ) + { + case POSERDATA_IMU: + { + PoserDataIMU * imu = (PoserDataIMU*)pd; + //printf( "IMU:%s (%f %f %f) (%f %f %f)\n", so->codename, imu->accel[0], imu->accel[1], imu->accel[2], imu->gyro[0], imu->gyro[1], imu->gyro[2] ); + break; + } + case POSERDATA_LIGHT: + { + PoserDataLight * l = (PoserDataLight*)pd; + //printf( "LIG:%s %d @ %f rad, %f s (AC %d) (TC %d)\n", so->codename, l->sensor_id, l->angle, l->length, l->acode, l->timecode ); + break; + } + case POSERDATA_FULL_SCENE: + { + PoserDataFullScene * fs = (PoserDataFullScene*)pd; + //printf( "Full scene data.\n" ); + break; + } + case POSERDATA_DISASSOCIATE: + { + free( dd ); + so->PoserData = 0; + //printf( "Need to disassociate.\n" ); + break; + } + } + +} + + +REGISTER_LINKTIME( PoserDummy ); + diff --git a/src/survive.c b/src/survive.c index 9bc1a2c..efa5d82 100644 --- a/src/survive.c +++ b/src/survive.c @@ -3,7 +3,6 @@ #include <survive.h> #include "survive_internal.h" -#include "survive_driverman.h" #include <stdio.h> #include <stdlib.h> #include <string.h> @@ -21,11 +20,11 @@ static void survivenote( struct SurviveContext * ctx, const char * fault ) } -struct SurviveContext * survive_init( int headless ) +SurviveContext * survive_init( int headless ) { int r = 0; int i = 0; - struct SurviveContext * ctx = calloc( 1, sizeof( struct SurviveContext ) ); + SurviveContext * ctx = calloc( 1, sizeof( SurviveContext ) ); ctx->faultfunction = survivefault; ctx->notefunction = survivenote; @@ -36,7 +35,6 @@ struct SurviveContext * survive_init( int headless ) const char * DriverName; while( ( DriverName = GetDriverNameMatching( "DriverReg", i++ ) ) ) - { DeviceDriver dd = GetDriver( DriverName ); printf( "Loading driver %s (%p) (%d)\n", DriverName, dd, i ); @@ -44,10 +42,34 @@ struct SurviveContext * survive_init( int headless ) printf( "Driver %s reports status %d\n", DriverName, r ); } + i = 0; + const char * PreferredPoser = "PoserDummy"; //config_read_str( cg, "DefualtPoser", "PoserDummy" ); /XXX Axlecrusher, can you add config stuff for this? + PoserCB PreferredPoserCB = 0; + const char * FirstPoser = 0; + printf( "Available posers:\n" ); + while( ( DriverName = GetDriverNameMatching( "Poser", i++ ) ) ) + { + PoserCB p = GetDriver( DriverName ); + if( !PreferredPoserCB ) PreferredPoserCB = p; + int ThisPoser = strcmp( DriverName, PreferredPoser ) == 0; + printf( "\t%c%s\n", ThisPoser?'*':' ', DriverName ); + if( ThisPoser ) PreferredPoserCB = p; + } + printf( "Totals %d posers. Using selected poser (or first!).\n", i-1 ); + if( !PreferredPoserCB ) + { + SV_ERROR( "Error. Cannot find any valid poser." ); + } + + for( i = 0; i < ctx->objs_ct; i++ ) + { + ctx->objs[i]->PoserFn = PreferredPoserCB; + } + return ctx; } -void survive_install_info_fn( struct SurviveContext * ctx, text_feedback_func fbp ) +void survive_install_info_fn( SurviveContext * ctx, text_feedback_func fbp ) { if( fbp ) ctx->notefunction = fbp; @@ -55,7 +77,7 @@ void survive_install_info_fn( struct SurviveContext * ctx, text_feedback_func f ctx->notefunction = survivenote; } -void survive_install_error_fn( struct SurviveContext * ctx, text_feedback_func fbp ) +void survive_install_error_fn( SurviveContext * ctx, text_feedback_func fbp ) { if( fbp ) ctx->faultfunction = fbp; @@ -63,7 +85,7 @@ void survive_install_error_fn( struct SurviveContext * ctx, text_feedback_func ctx->faultfunction = survivefault; } -void survive_install_light_fn( struct SurviveContext * ctx, light_process_func fbp ) +void survive_install_light_fn( SurviveContext * ctx, light_process_func fbp ) { if( fbp ) ctx->lightproc = fbp; @@ -71,7 +93,7 @@ void survive_install_light_fn( struct SurviveContext * ctx, light_process_func f ctx->lightproc = survive_default_light_process; } -void survive_install_imu_fn( struct SurviveContext * ctx, imu_process_func fbp ) +void survive_install_imu_fn( SurviveContext * ctx, imu_process_func fbp ) { if( fbp ) ctx->imuproc = fbp; @@ -80,7 +102,7 @@ void survive_install_imu_fn( struct SurviveContext * ctx, imu_process_func fbp } -void survive_install_angle_fn( struct SurviveContext * ctx, angle_process_func fbp ) +void survive_install_angle_fn( SurviveContext * ctx, angle_process_func fbp ) { if( fbp ) ctx->angleproc = fbp; @@ -88,15 +110,15 @@ void survive_install_angle_fn( struct SurviveContext * ctx, angle_process_func ctx->angleproc = survive_default_angle_process; } -int survive_add_object( struct SurviveContext * ctx, struct SurviveObject * obj ) +int survive_add_object( SurviveContext * ctx, SurviveObject * obj ) { int oldct = ctx->objs_ct; - ctx->objs = realloc( ctx->objs, sizeof( struct SurviveObject * ) * (oldct+1) ); + ctx->objs = realloc( ctx->objs, sizeof( SurviveObject * ) * (oldct+1) ); ctx->objs[oldct] = obj; ctx->objs_ct = oldct+1; } -void survive_add_driver( struct SurviveContext * ctx, void * payload, DeviceDriverCb poll, DeviceDriverCb close, DeviceDriverMagicCb magic ) +void survive_add_driver( SurviveContext * ctx, void * payload, DeviceDriverCb poll, DeviceDriverCb close, DeviceDriverMagicCb magic ) { int oldct = ctx->driver_ct; ctx->drivers = realloc( ctx->drivers, sizeof( void * ) * (oldct+1) ); @@ -110,7 +132,7 @@ void survive_add_driver( struct SurviveContext * ctx, void * payload, DeviceDriv ctx->driver_ct = oldct+1; } -int survive_send_magic( struct SurviveContext * ctx, int magic_code, void * data, int datalen ) +int survive_send_magic( SurviveContext * ctx, int magic_code, void * data, int datalen ) { int oldct = ctx->driver_ct; int i; @@ -120,7 +142,7 @@ int survive_send_magic( struct SurviveContext * ctx, int magic_code, void * data } } -void survive_close( struct SurviveContext * ctx ) +void survive_close( SurviveContext * ctx ) { const char * DriverName; int r = 0; @@ -134,6 +156,14 @@ void survive_close( struct SurviveContext * ctx ) int oldct = ctx->driver_ct; int i; + + for( i = 0; i < ctx->objs_ct; i++ ) + { + PoserData pd; + pd.pt = POSERDATA_DISASSOCIATE; + if( ctx->objs[i]->PoserFn ) ctx->objs[i]->PoserFn( ctx->objs[i], &pd ); + } + for( i = 0; i < oldct; i++ ) { ctx->drivercloses[i]( ctx, ctx->drivers[i] ); diff --git a/src/survive_cal.c b/src/survive_cal.c index 33d1faa..2acf51c 100644 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -27,8 +27,8 @@ static void reset_calibration( struct SurviveCalData * cd ); void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet) { - struct SurviveContext * ctx = (struct SurviveContext*)(ct->user); - struct SurviveCalData * cd = ctx->calptr; + SurviveContext * ctx = (SurviveContext*)(ct->user); + SurviveCalData * cd = ctx->calptr; int id = ct->user1; SV_INFO( "Got OOTX packet %d %p", id, cd ); @@ -36,7 +36,7 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet) lighthouse_info_v6 v6; init_lighthouse_info_v6(&v6, packet->data); - struct BaseStationData * b = &ctx->bsd[id]; + BaseStationData * b = &ctx->bsd[id]; //print_lighthouse_info_v6(&v6); b->BaseStationID = v6.id; diff --git a/src/survive_cal.h b/src/survive_cal.h index bb4eb32..dd2a1e2 100644 --- a/src/survive_cal.h +++ b/src/survive_cal.h @@ -2,6 +2,8 @@ // All OOTX code was written by J. Allen. Rest of the code is probably mostly CNLohr. +//XXX XXX XXX Warning: This subsystem will likely be mostly re-written. + #ifndef _SURVIVE_CAL_H #define _SURVIVE_CAL_H @@ -21,21 +23,21 @@ #include "ootx_decoder.h" #include "survive_internal.h" -void survive_cal_install( struct SurviveContext * ctx ); -int survive_cal_get_status( struct SurviveContext * ctx, char * description, int description_length ); +void survive_cal_install( SurviveContext * ctx ); +int survive_cal_get_status( SurviveContext * ctx, char * description, int description_length ); //void survive_cal_teardown( struct SurviveContext * ctx ); //Called from survive_default_light_process -void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); -void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); +void survive_cal_light( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); +void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); #define MAX_SENSORS_TO_CAL 96 #define DRPTS 1024 #define MAX_CAL_PT_DAT (MAX_SENSORS_TO_CAL*NUM_LIGHTHOUSES*2) struct SurviveCalData { - struct SurviveContext * ctx; + SurviveContext * ctx; //OOTX Data is sync'd off of the sync pulses coming from the lighthouses. ootx_decoder_context ootx_decoders[NUM_LIGHTHOUSES]; @@ -57,7 +59,7 @@ struct SurviveCalData int senid_of_checkpt; //This is a point on a watchman that can be used to check the lh solution. - struct SurviveObject * hmd; + SurviveObject * hmd; //Stage: // 0: Idle @@ -68,7 +70,7 @@ struct SurviveCalData //The following function is not included in the core survive_cal and must be compiled from a camfind file. //It should use data for stage 4 and report if it found the -int survive_cal_lhfind( struct SurviveCalData * cd ); +int survive_cal_lhfind( SurviveCalData * cd ); #endif diff --git a/src/survive_cal_lhfind.c b/src/survive_cal_lhfind.c index 93d9dc0..a1bb2cc 100644 --- a/src/survive_cal_lhfind.c +++ b/src/survive_cal_lhfind.c @@ -7,13 +7,13 @@ #define MIN_HITS_FOR_VALID 10 -FLT static RunOpti( struct SurviveCalData * cd, int lh, int print, FLT * LighthousePos, FLT * LighthouseQuat ); +FLT static RunOpti( SurviveCalData * cd, int lh, int print, FLT * LighthousePos, FLT * LighthouseQuat ); //Values used for RunTest() -int survive_cal_lhfind( struct SurviveCalData * cd ) +int survive_cal_lhfind( SurviveCalData * cd ) { - struct SurviveContext * ctx = cd->ctx; + SurviveContext * ctx = cd->ctx; int cycle, i; int lh = 0; FLT dx, dy, dz; @@ -137,8 +137,8 @@ int survive_cal_lhfind( struct SurviveCalData * cd ) } cd->ctx->bsd[lh].PositionSet = 1; - copy3d( cd->ctx->bsd[lh].Position, LighthousePos ); - quatcopy( cd->ctx->bsd[lh].Quaternion, LighthouseQuat ); + copy3d( cd->ctx->bsd[lh].Pose.Pos, LighthousePos ); + quatcopy( cd->ctx->bsd[lh].Pose.Rot, LighthouseQuat ); } return 0; //Return 0 if success. @@ -149,14 +149,14 @@ int survive_cal_lhfind( struct SurviveCalData * cd ) -static FLT RunOpti( struct SurviveCalData * cd, int lh, int print, FLT * LighthousePos, FLT * LighthouseQuat ) +static FLT RunOpti( SurviveCalData * cd, int lh, int print, FLT * LighthousePos, FLT * LighthouseQuat ) { int i, p; FLT UsToTarget[3]; FLT LastUsToTarget[3]; FLT mux = .9; quatsetnone( LighthouseQuat ); - struct SurviveObject * hmd = cd->hmd; + SurviveObject * hmd = cd->hmd; FLT * hmd_points = hmd->sensor_locations; FLT * hmd_normals = hmd->sensor_normals; diff --git a/src/survive_config.c b/src/survive_config.c index 8e0241a..4de6a3a 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -66,12 +66,11 @@ void config_init() { } -void config_set_lighthouse(struct BaseStationData* bsd, uint8_t idx) { +void config_set_lighthouse(BaseStationData* bsd, uint8_t idx) { config_group *cg = lh_config+idx; config_set_uint32(cg,"index", idx); config_set_uint32(cg,"id", bsd->BaseStationID); - config_set_float_a(cg,"position", bsd->Position, 3); - config_set_float_a(cg,"quaternion", bsd->Quaternion, 4); + config_set_float_a(cg,"pose", &bsd->Pose.Pos[0], 7); config_set_float_a(cg,"fcalphase", bsd->fcalphase, 2); config_set_float_a(cg,"fcaltilt", bsd->fcaltilt,2); config_set_float_a(cg,"fcalcurve", bsd->fcalcurve,2); diff --git a/src/survive_config.h b/src/survive_config.h index 234db68..cd16c76 100644 --- a/src/survive_config.h +++ b/src/survive_config.h @@ -40,7 +40,7 @@ void config_init(); //void config_open(const char* path, const char* mode); void config_read(const char* path); //void config_write_lighthouse(struct BaseStationData* bsd, uint8_t length); -void config_set_lighthouse(struct BaseStationData* bsd, uint8_t idx); +void config_set_lighthouse(BaseStationData* bsd, uint8_t idx); void config_save(const char* path); const FLT config_set_float(config_group *cg, const char *tag, const FLT value); @@ -52,4 +52,4 @@ uint16_t config_read_float_array(config_group *cg, const char *tag, FLT** values uint32_t config_read_uint32(config_group *cg, const char *tag, const uint32_t def); const char* config_read_str(config_group *cg, const char *tag, const char *def); -#endif
\ No newline at end of file +#endif diff --git a/src/survive_data.c b/src/survive_data.c index ac640c6..63cc5c2 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -6,9 +6,9 @@ #include <string.h> //This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode. -void handle_lightcap( struct SurviveObject * so, struct LightcapElement * le ) +void handle_lightcap( SurviveObject * so, LightcapElement * le ) { - struct SurviveContext * ctx = so->ctx; + SurviveContext * ctx = so->ctx; //int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time; //if( so->codename[0] != 'H' ) diff --git a/src/survive_driverman.c b/src/survive_driverman.c index 8cdfb71..d694e64 100644 --- a/src/survive_driverman.c +++ b/src/survive_driverman.c @@ -3,7 +3,7 @@ // See notice in survive_driverman.h // -#include "survive_driverman.h" +#include "survive_internal.h" #include <string.h> #include <stdio.h> diff --git a/src/survive_driverman.h b/src/survive_driverman.h deleted file mode 100644 index 5e13caf..0000000 --- a/src/survive_driverman.h +++ /dev/null @@ -1,38 +0,0 @@ -// (C) 2017 <>< C. N. Lohr, Under MIT/x11 License. -// -// This file is intended to be used for self-registering functions. By using -// this it means that you do not need to have complicated switch statements or -// #defines for dfferent inclusion of drivers/other code. You can simply -// register your function and it will be put into a list. -// -// - -#ifndef SURVIVE_DRIVERMAN_H -#define SURVIVE_DRIVERMAN_H - -//Driver registration -#define MAX_DRIVERS 32 - -void RegisterDriver( const char * name, void * data ); -void * GetDriver( const char * name ); -const char * GetDriverNameMatching( const char * prefix, int place ); -void ListDrivers(); - -#define REGISTER_LINKTIME( func ) \ - void __attribute__((constructor)) Register##func() { RegisterDriver( #func, &func ); } - - -// -// Specific types of drivers. -// - -struct SurviveContext; - -//Device drivers (prefix your drivers with "DriverReg") i.e. -// REGISTER_LINKTIME( DriverRegHTCVive ); -typedef int (*DeviceDriver)( struct SurviveContext * ctx ); - -//more driver types here? i.e. posefinders, etc. - -#endif - diff --git a/src/survive_internal.h b/src/survive_internal.h index 9d04d93..392104a 100644 --- a/src/survive_internal.h +++ b/src/survive_internal.h @@ -1,15 +1,5 @@ //<>< (C) 2016-2017 C. N. Lohr, MOSTLY Under MIT/x11 License. // -//Based off of https://github.com/collabora/OSVR-Vive-Libre -// Originally Copyright 2016 Philipp Zabel -// Originally Copyright 2016 Lubosz Sarnecki <lubosz.sarnecki@collabora.co.uk> -// Originally Copyright (C) 2013 Fredrik Hultin -// Originally Copyright (C) 2013 Jakob Bornecrantz -// -//But, re-written as best as I can to get it put under an open souce license instead of a forced-source license. -//If there are portions of the code too similar to the original, I would like to know so they can be re-written. -//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses. - #ifndef _SURVIVE_INTERNAL_H #define _SURVIVE_INTERNAL_H @@ -17,85 +7,16 @@ #include <stdlib.h> #include <stdio.h> #include <stdint.h> -#include "survive_driverman.h" #include <zlib.h> #include <survive.h> -#define SV_INFO( x... ) { char stbuff[1024]; sprintf( stbuff, x ); ctx->notefunction( ctx, stbuff ); } -#define SV_ERROR( x... ) { char stbuff[1024]; sprintf( stbuff, x ); ctx->faultfunction( ctx, stbuff ); } - -//XXX TODO This one needs to be rewritten. -#define SV_KILL() exit(0) - - -struct SurviveContext; -struct SurviveUSBInterface; - -typedef int (*DeviceDriverCb)( struct SurviveContext * ctx, void * driver ); -typedef int (*DeviceDriverMagicCb)( struct SurviveContext * ctx, void * driver, int magic_code, void * data, int datalen ); - -//This is defined in survive.h -struct SurviveObject; -struct SurviveCalData; - -struct BaseStationData -{ - uint8_t PositionSet:1; - FLT Position[3]; - FLT Quaternion[4]; - - uint8_t OOTXSet:1; - uint32_t BaseStationID; - FLT fcalphase[2]; - FLT fcaltilt[2]; - FLT fcalcurve[2]; - FLT fcalgibpha[2]; - FLT fcalgibmag[2]; -}; - -struct SurviveContext -{ - text_feedback_func faultfunction; - text_feedback_func notefunction; - light_process_func lightproc; - imu_process_func imuproc; - angle_process_func angleproc; - - //Calibration data: - struct BaseStationData bsd[NUM_LIGHTHOUSES]; - - struct SurviveCalData * calptr; //If and only if the calibration subsystem is attached. - - struct SurviveObject ** objs; - int objs_ct; - - void ** drivers; - DeviceDriverCb * driverpolls; - DeviceDriverCb * drivercloses; - DeviceDriverMagicCb * drivermagics; - int driver_ct; -}; - -int survive_add_object( struct SurviveContext * ctx, struct SurviveObject * obj ); - -void survive_add_driver( struct SurviveContext * ctx, void * payload, DeviceDriverCb poll, DeviceDriverCb close, DeviceDriverMagicCb magic ); - -//For lightcap, etc. Don't change this structure at all. Regular vive is dependent on it being exactly as-is. -struct LightcapElement -{ - uint8_t sensor_id; - uint8_t type; - uint16_t length; - uint32_t timestamp; -} __attribute__((packed)); - -//This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode. -void handle_lightcap( struct SurviveObject * so, struct LightcapElement * le ); - -//Accept Data from backend. -void survive_data_cb( struct SurviveUSBInterface * si ); +//Driver registration +#define MAX_DRIVERS 32 +void * GetDriver( const char * name ); +const char * GetDriverNameMatching( const char * prefix, int place ); +void ListDrivers(); #endif diff --git a/src/survive_process.c b/src/survive_process.c index 75453da..2fea99d 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -6,9 +6,9 @@ //XXX TODO: Once data is avialble in the context, use the stuff here to handle converting from time codes to //proper angles, then from there perform the rest of the solution. -void survive_default_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ) +void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ) { - struct SurviveContext * ctx = so->ctx; + SurviveContext * ctx = so->ctx; int base_station = acode >> 2; int axis = acode & 1; @@ -25,7 +25,7 @@ void survive_default_light_process( struct SurviveObject * so, int sensor_id, in //Need to now do angle correction. #if 1 - struct BaseStationData * bsd = &ctx->bsd[base_station]; + BaseStationData * bsd = &ctx->bsd[base_station]; //XXX TODO: This seriously needs to be worked on. See: https://github.com/cnlohr/libsurvive/issues/18 angle += bsd->fcalphase[axis]; @@ -39,20 +39,41 @@ void survive_default_light_process( struct SurviveObject * so, int sensor_id, in } -void survive_default_angle_process( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ) +void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ) { - struct SurviveContext * ctx = so->ctx; + SurviveContext * ctx = so->ctx; if( ctx->calptr ) { survive_cal_angle( so, sensor_id, acode, timecode, length, angle ); } - - //TODO: Writeme! + if( so->PoserFn ) + { + PoserDataLight l = { + .pt = POSERDATA_LIGHT, + .sensor_id = sensor_id, + .acode = acode, + .timecode = timecode, + .length = length, + .angle = angle, + }; + so->PoserFn( so, (PoserData *)&l ); + } } -void survive_default_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ) +void survive_default_imu_process( SurviveObject * so, int mask, FLT * accelgyromag, uint32_t timecode, int id ) { - //TODO: Writeme! + if( so->PoserFn ) + { + PoserDataIMU imu = { + .pt = POSERDATA_IMU, + .datamask = mask, + .accel = { accelgyromag[0], accelgyromag[1], accelgyromag[2] }, + .gyro = { accelgyromag[3], accelgyromag[4], accelgyromag[5] }, + .mag = { accelgyromag[6], accelgyromag[7], accelgyromag[8] }, + .timecode = timecode, + }; + so->PoserFn( so, (PoserData *)&imu ); + } } diff --git a/src/survive_vive.c b/src/survive_vive.c index 47886c9..7da2897 100644 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -10,27 +10,18 @@ //If there are portions of the code too similar to the original, I would like to know so they can be re-written. //All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses. -#include "survive_internal.h" -#include "survive_driverman.h" +#include <survive.h> #include <jsmn.h> - -#include "survive_internal.h" #include <libusb-1.0/libusb.h> #include <stdio.h> -#include <unistd.h> //sleep if I ever use it. +#include <stdlib.h> +#include <unistd.h> #include <errno.h> #include <string.h> #include <sys/stat.h> struct SurviveViveData; -//USB Subsystem -void survive_usb_close( struct SurviveContext * t ); -int survive_usb_init( struct SurviveViveData * sv, struct SurviveObject * hmd, struct SurviveObject *wm0, struct SurviveObject * wm1 ); -int survive_usb_poll( struct SurviveContext * ctx ); -int survive_get_config( char ** config, struct SurviveViveData * ctx, int devno, int interface, int send_extra_magic ); - - const short vidpids[] = { 0x0bb4, 0x2c87, 0, //The main HTC HMD device 0x28de, 0x2000, 0, //Valve lighthouse @@ -60,15 +51,18 @@ const char * devnames[] = { #define USB_IF_LIGHTCAP 4 #define MAX_INTERFACES 5 -typedef void (*usb_callback)( struct SurviveUSBInterface * ti ); +typedef struct SurviveUSBInterface SurviveUSBInterface; +typedef struct SurviveViveData SurviveViveData; + +typedef void (*usb_callback)( SurviveUSBInterface * ti ); struct SurviveUSBInterface { - struct SurviveViveData * sv; - struct SurviveContext * ctx; + SurviveViveData * sv; + SurviveContext * ctx; struct libusb_transfer * transfer; - struct SurviveObject * assoc_obj; + SurviveObject * assoc_obj; int actual_len; uint8_t buffer[INTBUFFSIZE]; usb_callback cb; @@ -78,16 +72,24 @@ struct SurviveUSBInterface struct SurviveViveData { - struct SurviveContext * ctx; + SurviveContext * ctx; //XXX TODO: UN-STATICIFY THIS. - struct SurviveUSBInterface uiface[MAX_INTERFACES]; + SurviveUSBInterface uiface[MAX_INTERFACES]; //USB Subsystem struct libusb_context* usbctx; struct libusb_device_handle * udev[MAX_USB_DEVS]; }; +void survive_data_cb( SurviveUSBInterface * si ); + +//USB Subsystem +void survive_usb_close( SurviveContext * t ); +int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *wm0, SurviveObject * wm1 ); +int survive_usb_poll( SurviveContext * ctx ); +int survive_get_config( char ** config, SurviveViveData * ctx, int devno, int interface, int send_extra_magic ); + @@ -115,10 +117,10 @@ static void handle_transfer(struct libusb_transfer* transfer) -static int AttachInterface( struct SurviveViveData * sv, struct SurviveObject * assocobj, int which_interface_am_i, libusb_device_handle * devh, int endpoint, usb_callback cb, const char * hname ) +static int AttachInterface( SurviveViveData * sv, SurviveObject * assocobj, int which_interface_am_i, libusb_device_handle * devh, int endpoint, usb_callback cb, const char * hname ) { - struct SurviveContext * ctx = sv->ctx; - struct SurviveUSBInterface * iface = &sv->uiface[which_interface_am_i]; + SurviveContext * ctx = sv->ctx; + SurviveUSBInterface * iface = &sv->uiface[which_interface_am_i]; iface->ctx = ctx; iface->sv = sv; iface->which_interface_am_i = which_interface_am_i; @@ -593,7 +595,13 @@ static void handle_watchman( struct 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; - w->ctx->imuproc( w, (int16_t *)&readdata[1], (time1<<24)|(time2<<16)|readdata[0], 0 ); + //XXX XXX BIG TODO!!! Actually recal gyro data. + FLT agm[9] = { readdata[1], readdata[2], readdata[3], + readdata[4], readdata[5], readdata[6], + 0,0,0 }; + + w->ctx->imuproc( w, 3, agm, (time1<<24)|(time2<<16)|readdata[0], 0 ); + int16_t * k = (int16_t *)readdata+1; //printf( "Match8 %d %d %d %d %d %3d %3d\n", qty, k[0], k[1], k[2], k[3], k[4], k[5] ); readdata += 13; qty -= 13; @@ -680,7 +688,7 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata ) } - struct LightcapElement les[10]; + LightcapElement les[10]; int lese = 0; //les's end //Second, go through all LEDs and extract the lightevent from them. @@ -711,7 +719,7 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata ) //reverse sorted, but that is to minimize operations. To read it //in sorted order simply read it back backwards. //Use insertion sort, since we should most of the time, be in order. - struct LightcapElement * le = &les[lese++]; + LightcapElement * le = &les[lese++]; le->sensor_id = led; le->type = 0xfe; @@ -725,7 +733,7 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata ) int swap = lese-2; while( swap >= 0 && les[swap].timestamp < les[swap+1].timestamp ) { - struct LightcapElement l; + LightcapElement l; memcpy( &l, &les[swap], sizeof( l ) ); memcpy( &les[swap], &les[swap+1], sizeof( l ) ); memcpy( &les[swap+1], &l, sizeof( l ) ); @@ -744,17 +752,17 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata ) return; end: { - struct SurviveContext * ctx = w->ctx; + SurviveContext * ctx = w->ctx; SV_INFO( "Light decoding fault: %d\n", fault ); } } } -void survive_data_cb( struct SurviveUSBInterface * si ) +void survive_data_cb( SurviveUSBInterface * si ) { int size = si->actual_len; - struct SurviveContext * ctx = si->ctx; + SurviveContext * ctx = si->ctx; #if 0 int i; printf( "%16s: %d: ", si->hname, len ); @@ -767,7 +775,7 @@ void survive_data_cb( struct SurviveUSBInterface * si ) #endif int iface = si->which_interface_am_i; - struct SurviveObject * obj = si->assoc_obj; + SurviveObject * obj = si->assoc_obj; uint8_t * readdata = si->buffer; int id = POP1; @@ -808,7 +816,13 @@ void survive_data_cb( struct SurviveUSBInterface * si ) if( cd > 0 ) { obj->oldcode = code; - ctx->imuproc( obj, acceldata, timecode, code ); + + //XXX XXX BIG TODO!!! Actually recal gyro data. + FLT agm[9] = { acceldata[0], acceldata[1], acceldata[2], + acceldata[3], acceldata[4], acceldata[5], + 0,0,0 }; + + ctx->imuproc( obj, 3, agm, timecode, code ); } } @@ -818,7 +832,7 @@ void survive_data_cb( struct SurviveUSBInterface * si ) case USB_IF_WATCHMAN1: case USB_IF_WATCHMAN2: { - struct SurviveObject * w = obj; + SurviveObject * w = obj; if( id == 35 ) { handle_watchman( w, readdata); @@ -844,7 +858,7 @@ void survive_data_cb( struct SurviveUSBInterface * si ) int i; for( i = 0; i < 7; i++ ) { - handle_lightcap( obj, (struct LightcapElement*)&readdata[i*8] ); + handle_lightcap( obj, (LightcapElement*)&readdata[i*8] ); } break; } @@ -875,7 +889,7 @@ static int jsoneq(const char *json, jsmntok_t *tok, const char *s) { } -static int ParsePoints( struct SurviveContext * ctx, struct SurviveObject * so, char * ct0conf, FLT ** floats_out, jsmntok_t * t, int i ) +static int ParsePoints( SurviveContext * ctx, SurviveObject * so, char * ct0conf, FLT ** floats_out, jsmntok_t * t, int i ) { int k; int pts = t[i+1].size; @@ -914,9 +928,9 @@ static int ParsePoints( struct SurviveContext * ctx, struct SurviveObject * so, return 0; } -static int LoadConfig( struct SurviveViveData * sv, struct SurviveObject * so, int devno, int iface, int extra_magic ) +static int LoadConfig( SurviveViveData * sv, SurviveObject * so, int devno, int iface, int extra_magic ) { - struct SurviveContext * ctx = sv->ctx; + SurviveContext * ctx = sv->ctx; char * ct0conf = 0; int len = survive_get_config( &ct0conf, sv, devno, iface, extra_magic ); @@ -1002,21 +1016,21 @@ static int LoadConfig( struct SurviveViveData * sv, struct SurviveObject * so, i } -int survive_vive_close( struct SurviveContext * ctx, void * driver ) +int survive_vive_close( SurviveContext * ctx, void * driver ) { - struct SurviveViveData * sv = driver; + SurviveViveData * sv = driver; survive_vive_usb_close( sv ); } -int DriverRegHTCVive( struct SurviveContext * ctx ) +int DriverRegHTCVive( SurviveContext * ctx ) { int i, r; - struct SurviveObject * hmd = calloc( 1, sizeof( struct SurviveObject ) ); - struct SurviveObject * wm0 = calloc( 1, sizeof( struct SurviveObject ) ); - struct SurviveObject * wm1 = calloc( 1, sizeof( struct SurviveObject ) ); - struct SurviveViveData * sv = calloc( 1, sizeof( struct SurviveViveData ) ); + SurviveObject * hmd = calloc( 1, sizeof( SurviveObject ) ); + SurviveObject * wm0 = calloc( 1, sizeof( SurviveObject ) ); + SurviveObject * wm1 = calloc( 1, sizeof( SurviveObject ) ); + SurviveViveData * sv = calloc( 1, sizeof( SurviveViveData ) ); sv->ctx = ctx; |