diff options
| -rw-r--r-- | Makefile | 2 | ||||
| -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 | 4 | ||||
| -rw-r--r-- | include/libsurvive/survive.h | 4 | ||||
| -rw-r--r-- | include/libsurvive/survive_types.h | 2 | ||||
| -rw-r--r-- | src/survive.c | 57 | ||||
| -rw-r--r-- | src/survive_cal.h | 2 | ||||
| -rw-r--r-- | src/survive_process.c | 29 | ||||
| -rw-r--r-- | src/survive_vive.c | 16 |
11 files changed, 103 insertions, 37 deletions
@@ -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 ); } } |