Merge branch 'master' of github.com:cnlohr/libsurvive

3 files changed, 128 insertions, 40 deletions

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