diff options
Diffstat (limited to 'src/survive_vive.c')
-rw-r--r-- | src/survive_vive.c | 120 |
1 files changed, 78 insertions, 42 deletions
diff --git a/src/survive_vive.c b/src/survive_vive.c index a402153..7da2897 100644 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -10,26 +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 @@ -59,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; @@ -77,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 ); + @@ -114,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; @@ -592,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; @@ -679,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. @@ -710,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; @@ -724,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 ) ); @@ -743,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 ); @@ -766,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; @@ -807,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 ); } } @@ -817,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); @@ -843,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; } @@ -874,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; @@ -913,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 ); @@ -976,25 +991,46 @@ static int LoadConfig( struct SurviveViveData * sv, struct SurviveObject * so, i //TODO: Cleanup any remaining USB stuff. return 1; } + + char fname[20]; + mkdir( "calinfo", 0755 ); + + sprintf( fname, "calinfo/%s_points.csv", so->codename ); + FILE * f = fopen( fname, "w" ); + int j; + for( j = 0; j < so->nr_locations; j++ ) + { + fprintf( f, "%f %f %f\n", so->sensor_locations[j*3+0], so->sensor_locations[j*3+1], so->sensor_locations[j*3+2] ); + } + fclose( f ); + + sprintf( fname, "calinfo/%s_normals.csv", so->codename ); + f = fopen( fname, "w" ); + for( j = 0; j < so->nr_locations; j++ ) + { + fprintf( f, "%f %f %f\n", so->sensor_normals[j*3+0], so->sensor_normals[j*3+1], so->sensor_normals[j*3+2] ); + } + fclose( f ); + return 0; } -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; |