From 394cbc465e776137834eea830038b43ea98f6268 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Wed, 8 Mar 2017 00:30:48 -0500 Subject: Switch types over to avoiding extra struct keyword. Switch poses to "SurvivePose" type. --- include/libsurvive/poser.h | 22 +++++++++++----------- include/libsurvive/survive.h | 8 ++++---- include/libsurvive/survive_types.h | 17 +++++++++++------ 3 files changed, 26 insertions(+), 21 deletions(-) (limited to 'include') diff --git a/include/libsurvive/poser.h b/include/libsurvive/poser.h index 4894acf..c514953 100644 --- a/include/libsurvive/poser.h +++ b/include/libsurvive/poser.h @@ -11,22 +11,22 @@ typedef enum PoserType_t POSERDATA_FULL_SCENE, //Full, statified X, Y sweep data for both lighthouses. } 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]; -}; +} PoserDataIMU; -struct PoserDataLight +typedef struct { PoserType pt; int sensor_id; @@ -34,21 +34,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 ); +typedef int (*PoserCB)( SurviveObject * so, PoserData * pd ); #endif diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 536c7fb..20e50ca 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -4,14 +4,14 @@ #include #include "poser.h" -struct SurviveContext; +typedef struct SurviveContext 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" @@ -27,8 +27,8 @@ struct SurviveObject //Pose Information, also "resolver" 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; diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h index 593819f..6137774 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,13 @@ #define INTBUFFSIZE 64 #define SENSORS_PER_OBJECT 32 -struct SurviveObject; -struct SurviveContext; +typedef struct SurviveObject SurviveObject; +typedef struct SurviveContext SurviveContext; -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 ); +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, int16_t * 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 ); #endif -- cgit v1.3.1 From a087915c5dd8c356911453a019723ebaa9904a36 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Wed, 8 Mar 2017 00:53:28 -0500 Subject: Move most of the libsurvive stuff into a common area. --- include/libsurvive/survive.h | 101 ++++++++++++++++++++++++++++++------- include/libsurvive/survive_types.h | 10 ++++ src/survive_driverman.h | 24 ++------- src/survive_internal.h | 75 --------------------------- 4 files changed, 98 insertions(+), 112 deletions(-) (limited to 'include') diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 20e50ca..01b5e2b 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -2,10 +2,9 @@ #define _SURVIVE_H #include +#include "survive_types.h" #include "poser.h" -typedef struct SurviveContext 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. @@ -25,7 +24,7 @@ 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 SurvivePose OutPose; SurvivePose FromLHPose[NUM_LIGHTHOUSES]; //Optionally filled out by poser, contains computed position from each lighthouse. @@ -62,34 +61,102 @@ struct SurviveObject int tsl; }; -struct SurviveContext * survive_init( int headless ); + +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 ); //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 ); +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( struct SurviveContext * ctx ); +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 ); //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, int16_t * 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 ); +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 ); } + +int survive_add_object( SurviveContext * ctx, SurviveObject * obj ); +void survive_add_driver( SurviveContext * ctx, void * payload, DeviceDriverCb poll, DeviceDriverCb close, DeviceDriverMagicCb magic ); + + +////////////////////// Lightcap driver data /////////////////////// + +//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 ); #endif diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h index 6137774..f235330 100644 --- a/include/libsurvive/survive_types.h +++ b/include/libsurvive/survive_types.h @@ -24,11 +24,21 @@ typedef struct SurvivePose 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, int16_t * 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 ); + + #endif diff --git a/src/survive_driverman.h b/src/survive_driverman.h index 5e13caf..216333a 100644 --- a/src/survive_driverman.h +++ b/src/survive_driverman.h @@ -10,29 +10,13 @@ #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 ); } - +#include "survive.h" -// -// Specific types of drivers. -// - -struct SurviveContext; +//Very little here. Mostly included in survive.h. -//Device drivers (prefix your drivers with "DriverReg") i.e. -// REGISTER_LINKTIME( DriverRegHTCVive ); -typedef int (*DeviceDriver)( struct SurviveContext * ctx ); +//Driver registration +#define MAX_DRIVERS 32 -//more driver types here? i.e. posefinders, etc. #endif diff --git a/src/survive_internal.h b/src/survive_internal.h index 83a429c..ff156ad 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 -// 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 @@ -28,71 +18,6 @@ #define SV_KILL() exit(0) -struct SurviveContext; -struct SurvivecalData; - -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 -typedef struct SurviveObject SurviveObject; -typedef struct SurviveCalData SurviveCalData; - -typedef struct -{ - 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]; -} BaseStationData; - -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; -}; - -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. -typedef struct -{ - uint8_t sensor_id; - uint8_t type; - 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 ); - - #endif -- cgit v1.3.1 From 2a4a803b2f162692ca11e700b32da0a77049bfd2 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Wed, 8 Mar 2017 01:01:42 -0500 Subject: cleaning up more header stuff --- include/libsurvive/survive.h | 18 +++++++++++------- src/survive.c | 1 - src/survive_driverman.c | 2 +- src/survive_driverman.h | 22 ---------------------- src/survive_internal.h | 10 +++++----- src/survive_vive.c | 8 +++----- 6 files changed, 20 insertions(+), 41 deletions(-) delete mode 100644 src/survive_driverman.h (limited to 'include') diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 01b5e2b..c35e43c 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -103,6 +103,7 @@ struct SurviveContext SurviveContext * survive_init( int headless ); //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 ); @@ -120,7 +121,7 @@ int survive_simple_inflate( SurviveContext * ctx, const char * input, int inlen, int survive_send_magic( SurviveContext * ctx, int magic_code, void * data, int datalen ); //Install the calibrator. -void survive_cal_install( 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. @@ -132,18 +133,17 @@ void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode ////////////////////// Survive Drivers //////////////////////////// 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 ); } -int survive_add_object( SurviveContext * ctx, SurviveObject * obj ); -void survive_add_driver( SurviveContext * ctx, void * payload, DeviceDriverCb poll, DeviceDriverCb close, DeviceDriverMagicCb magic ); -////////////////////// Lightcap driver data /////////////////////// +///////////////////////// 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. @@ -158,5 +158,9 @@ typedef struct //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/src/survive.c b/src/survive.c index 9bc1a2c..e1ab943 100644 --- a/src/survive.c +++ b/src/survive.c @@ -3,7 +3,6 @@ #include #include "survive_internal.h" -#include "survive_driverman.h" #include #include #include 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 #include diff --git a/src/survive_driverman.h b/src/survive_driverman.h deleted file mode 100644 index 216333a..0000000 --- a/src/survive_driverman.h +++ /dev/null @@ -1,22 +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 - -#include "survive.h" - -//Very little here. Mostly included in survive.h. - -//Driver registration -#define MAX_DRIVERS 32 - - -#endif - diff --git a/src/survive_internal.h b/src/survive_internal.h index ff156ad..392104a 100644 --- a/src/survive_internal.h +++ b/src/survive_internal.h @@ -7,16 +7,16 @@ #include #include #include -#include "survive_driverman.h" #include #include -#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) +//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_vive.c b/src/survive_vive.c index 6be5114..61716d3 100644 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -10,14 +10,12 @@ //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 #include - -#include "survive_internal.h" #include #include -#include //sleep if I ever use it. +#include +#include #include #include #include -- cgit v1.3.1 From c65498054c77192b2a12fdb5ef44439a14110292 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Wed, 8 Mar 2017 01:43:56 -0500 Subject: Architect the way the posers receive data. --- Makefile | 2 +- calibrate.c | 9 +++--- calibrate_client.c | 9 +++--- data_recorder.c | 6 ++-- include/libsurvive/poser.h | 4 ++- include/libsurvive/survive.h | 4 +-- include/libsurvive/survive_types.h | 2 +- src/survive.c | 57 +++++++++++++++++++++++++++++--------- src/survive_cal.h | 2 ++ src/survive_process.c | 29 ++++++++++++++++--- src/survive_vive.c | 16 +++++++++-- 11 files changed, 103 insertions(+), 37 deletions(-) (limited to 'include') diff --git a/Makefile b/Makefile index 470cbe3..3c8431b 100644 --- a/Makefile +++ b/Makefile @@ -23,7 +23,7 @@ calibrate_client : calibrate_client.c lib/libsurvive.so redist/os_generic.c red 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 : 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 src/PoserDummy.o $(DEBUGSTUFF) $(CALS) 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 c514953..98c926e 100644 --- a/include/libsurvive/poser.h +++ b/include/libsurvive/poser.h @@ -9,6 +9,7 @@ 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; typedef struct @@ -24,6 +25,7 @@ typedef struct FLT accel[3]; FLT gyro[3]; FLT mag[3]; + uint32_t timecode; //In object-local ticks. } PoserDataIMU; typedef struct @@ -47,7 +49,7 @@ typedef struct PoserDataIMU lastimu; } PoserDataFullScene; -//When you register your posers using the internal system, +//When you write your posers, use the following definition, and register with REGISTER_LINKTIME. typedef int (*PoserCB)( SurviveObject * so, PoserData * pd ); diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index c35e43c..7fd6046 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -29,7 +29,7 @@ struct SurviveObject 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; @@ -126,7 +126,7 @@ void survive_cal_install( SurviveContext * ctx ); //XXX This will be removed if //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 ); -void survive_default_imu_process( SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ); +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 ); diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h index f235330..1600e11 100644 --- a/include/libsurvive/survive_types.h +++ b/include/libsurvive/survive_types.h @@ -29,7 +29,7 @@ typedef struct SurviveCalData SurviveCalData; //XXX Warning: This may be remov 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, int16_t * accelgyro, uint32_t timecode, int id ); +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 ); diff --git a/src/survive.c b/src/survive.c index e1ab943..efa5d82 100644 --- a/src/survive.c +++ b/src/survive.c @@ -20,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; @@ -35,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 ); @@ -43,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; @@ -54,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; @@ -62,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; @@ -70,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; @@ -79,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; @@ -87,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) ); @@ -109,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; @@ -119,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; @@ -133,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.h b/src/survive_cal.h index a5e372e..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 diff --git a/src/survive_process.c b/src/survive_process.c index edabfff..2fea99d 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -46,13 +46,34 @@ void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode { 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( 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 61716d3..7da2897 100644 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -595,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; @@ -810,7 +816,13 @@ void survive_data_cb( 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 ); } } -- cgit v1.3.1