From a015a5bc3631f813281c5af85a29b9d84a3eb924 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Tue, 19 Dec 2017 23:30:52 -0700 Subject: Add support for using only 1 lighthouse Change adds a config option for the number of "active" lighthouses, which can be 1. Calibrate will run with a single lighthouse if the new config option LighthouseCount is set to 1. --- include/libsurvive/survive.h | 1 + 1 file changed, 1 insertion(+) (limited to 'include') diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 1532d62..c4b2abe 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -102,6 +102,7 @@ struct SurviveContext struct config_group* lh_config; //lighthouse configs //Calibration data: + int activeLighthouses; BaseStationData bsd[NUM_LIGHTHOUSES]; SurviveCalData * calptr; //If and only if the calibration subsystem is attached. -- cgit v1.3.1 From 04e44b9f1c1b65198ea1ac883dcd9f153933413d Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Tue, 2 Jan 2018 21:37:57 -0700 Subject: Capturing Button Inputs Currently only working on Windows over USB interface Inputs are only printed out, still need to propagate them up the stack. Buttons are placed in a queue, and "processed" on a different thread to avoid starvation. --- include/libsurvive/survive.h | 40 +++++++- src/survive.c | 55 ++++++++++ src/survive_vive.c | 233 ++++++++++++++++++++++++++++++++++++++----- 3 files changed, 301 insertions(+), 27 deletions(-) (limited to 'include') diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index c4b2abe..f3a17c8 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -4,6 +4,7 @@ #include #include "survive_types.h" #include "poser.h" +#include "os_generic.h" #ifdef __cplusplus extern "C" { @@ -18,7 +19,7 @@ struct SurviveObject char codename[4]; //3 letters, null-terminated. Currently HMD, WM0, WM1. char drivername[4]; //3 letters for driver. Currently "HTC" - int16_t buttonmask; + int32_t buttonmask; int16_t axis1; int16_t axis2; @@ -90,6 +91,37 @@ struct BaseStationData struct config_group; +#define BUTTON_QUEUE_MAX_LEN 32 + +#define BUTTON_EVENT_BUTTON_NONE 0 +#define BUTTON_EVENT_BUTTON_DOWN 1 +#define BUTTON_EVENT_BUTTON_UP 2 +#define BUTTON_EVENT_AXIS_CHANGED 3 + +// note: buttonId and axisId are 1-indexed values. +// a value of 0 for an id means that no data is present in that value +// additionally, when x and y values are both present in axis data, +// axis1 will be x, axis2 will be y. +typedef struct +{ + uint8_t isPopulated; //probably can remove this given the semaphore in the parent struct. helps with debugging + uint8_t eventType; + uint8_t buttonId; + uint8_t axis1Id; + uint16_t axis1Val; + uint8_t axis2Id; + uint16_t axis2Val; + SurviveObject *so; +} ButtonQueueEntry; + +typedef struct +{ + uint8_t nextReadIndex; //init to 0 + uint8_t nextWriteIndex; // init to 0 + og_sema_t buttonservicesem; + ButtonQueueEntry entry[BUTTON_QUEUE_MAX_LEN]; +} ButtonQueue; + struct SurviveContext { text_feedback_func faultfunction; @@ -114,6 +146,12 @@ struct SurviveContext DeviceDriverCb * drivercloses; DeviceDriverMagicCb * drivermagics; int driver_ct; + + uint8_t isClosing; // flag to indicate if threads should terminate themselves + + og_thread_t buttonservicethread; + ButtonQueue buttonQueue; + }; SurviveContext * survive_init( int headless ); diff --git a/src/survive.c b/src/survive.c index 97c839b..99b6648 100755 --- a/src/survive.c +++ b/src/survive.c @@ -40,6 +40,45 @@ static void survivenote( struct SurviveContext * ctx, const char * fault ) fprintf( stderr, "Info: %s\n", fault ); } +static void button_servicer(void * context) +{ + SurviveContext *ctx = (SurviveContext*)context; + + while (1) + { + OGLockSema(ctx->buttonQueue.buttonservicesem); + + if (ctx->isClosing) + { + // we're shutting down. Close. + return; + } + + ButtonQueueEntry *entry = &(ctx->buttonQueue.entry[ctx->buttonQueue.nextReadIndex]); + if (entry->isPopulated == 0) + { + // should never happen. indicates failure of code pushing stuff onto + // the buttonQueue + // if it does happen, it will kill all future button input + printf("ERROR: Unpopulated ButtonQueueEntry!"); + return; + } + + printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n", + entry->eventType, + entry->buttonId, + entry->axis1Id, + entry->axis1Val, + entry->axis2Id, + entry->axis2Val); + + ctx->buttonQueue.nextReadIndex++; + if (ctx->buttonQueue.nextReadIndex >= BUTTON_QUEUE_MAX_LEN) + { + ctx->buttonQueue.nextReadIndex = 0; + } + }; +} SurviveContext * survive_init( int headless ) { @@ -66,6 +105,8 @@ SurviveContext * survive_init( int headless ) int i = 0; SurviveContext * ctx = calloc( 1, sizeof( SurviveContext ) ); + ctx->isClosing = 0; + ctx->global_config_values = malloc( sizeof(config_group) ); ctx->lh_config = malloc( sizeof(config_group) * NUM_LIGHTHOUSES); @@ -123,6 +164,14 @@ SurviveContext * survive_init( int headless ) // saving the config extra to make sure that the user has a config file they can change. config_save(ctx, "config.json"); + // initialize the button queue + memset(&(ctx->buttonQueue), 0, sizeof(ctx->buttonQueue)); + + ctx->buttonQueue.buttonservicesem = OGCreateSema(); + + // start the thread to process button data + ctx->buttonservicethread = OGCreateThread(button_servicer, ctx); + return ctx; } @@ -205,6 +254,12 @@ void survive_close( SurviveContext * ctx ) { const char * DriverName; int r = 0; + + ctx->isClosing = 1; + + // unlock/ post to button service semaphore so the thread can kill itself + OGUnlockSema(ctx->buttonQueue.buttonservicesem); + while( ( DriverName = GetDriverNameMatching( "DriverUnreg", r++ ) ) ) { DeviceDriver dd = GetDriver( DriverName ); diff --git a/src/survive_vive.c b/src/survive_vive.c index ef150f6..146ea01 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -865,6 +865,162 @@ void calibrate_gyro(SurviveObject* so, FLT* agm) { } } +typedef struct +{ + // could use a bitfield here, but since this data is short-lived, + // the space savings probably isn't worth the processing overhead. + uint8_t pressedButtonsValid; + uint8_t triggerOfBatteryValid; + uint8_t batteryChargeValid; + uint8_t hardwareIdValid; + uint8_t touchpadHorizontalValid; + uint8_t touchpadVerticalValid; + uint8_t triggerHighResValid; + + uint32_t pressedButtons; + uint16_t triggerOrBattery; + uint8_t batteryCharge; + uint32_t hardwareId; + uint16_t touchpadHorizontal; + uint16_t touchpadVertical; + uint16_t triggerHighRes; +} buttonEvent; + +void incrementAndPostButtonQueue(SurviveContext *ctx) +{ + ButtonQueueEntry *entry = &(ctx->buttonQueue.entry[ctx->buttonQueue.nextWriteIndex]); + + if (OGGetSema(ctx->buttonQueue.buttonservicesem) >= BUTTON_QUEUE_MAX_LEN) + { + // There's not enough space to write this entry. Clear it out and move along + memset(entry, 0, sizeof(ButtonQueueEntry)); + return; + } + entry->isPopulated = 1; + ctx->buttonQueue.nextWriteIndex++; + // if we've exceeded the size of the buffer, loop around to the beginning. + if (ctx->buttonQueue.nextWriteIndex >= BUTTON_QUEUE_MAX_LEN) + { + ctx->buttonQueue.nextWriteIndex = 0; + } + OGUnlockSema(ctx->buttonQueue.buttonservicesem); + + // clear out any old data in the entry so we always start with a clean slate. + entry = &(ctx->buttonQueue.entry[ctx->buttonQueue.nextWriteIndex]); + memset(entry, 0, sizeof(ButtonQueueEntry)); +} + +// important! This must be the only place that we're posting to the buttonEntryQueue +// if that ever needs to be changed, you will have to add locking so that only one +// thread is posting at a time. +void registerButtonEvent(SurviveObject *so, buttonEvent *event) +{ + ButtonQueueEntry *entry = &(so->ctx->buttonQueue.entry[so->ctx->buttonQueue.nextWriteIndex]); + + memset(entry, 0, sizeof(ButtonQueueEntry)); + + entry->so = so; + if (event->pressedButtonsValid) + { + //printf("trigger %8.8x\n", event->triggerHighRes); + for (int a=0; a < 16; a++) + { + if (((event->pressedButtons) & (1<buttonmask) & (1<pressedButtons & (1 << a)) + { + // it went down + entry->eventType = BUTTON_EVENT_BUTTON_DOWN; + } + else + { + // it went up + entry->eventType = BUTTON_EVENT_BUTTON_UP; + } + entry->buttonId = a; + incrementAndPostButtonQueue(so->ctx); + entry = &(so->ctx->buttonQueue.entry[so->ctx->buttonQueue.nextWriteIndex]); + } + } + // if the trigger button is depressed & it wasn't before + if ((((event->pressedButtons) & (0xff000000)) == 0xff000000) && + ((so->buttonmask) & (0xff000000)) != 0xff000000) + { + entry->eventType = BUTTON_EVENT_BUTTON_DOWN; + entry->buttonId = 24; + incrementAndPostButtonQueue(so->ctx); + entry = &(so->ctx->buttonQueue.entry[so->ctx->buttonQueue.nextWriteIndex]); + } + // if the trigger button isn't depressed but it was before + else if ((((event->pressedButtons) & (0xff000000)) != 0xff000000) && + ((so->buttonmask) & (0xff000000)) == 0xff000000) + { + entry->eventType = BUTTON_EVENT_BUTTON_UP; + entry->buttonId = 24; + incrementAndPostButtonQueue(so->ctx); + entry = &(so->ctx->buttonQueue.entry[so->ctx->buttonQueue.nextWriteIndex]); + } + } + if (event->triggerHighResValid) + { + if (so->axis1 != event->triggerHighRes) + { + entry->eventType = BUTTON_EVENT_AXIS_CHANGED; + entry->axis1Id = 1; + entry->axis1Val = event->triggerHighRes; + incrementAndPostButtonQueue(so->ctx); + entry = &(so->ctx->buttonQueue.entry[so->ctx->buttonQueue.nextWriteIndex]); + + } + } + if ((event->touchpadHorizontalValid) && (event->touchpadVerticalValid)) + { + if ((so->axis2 != event->touchpadHorizontal) || + (so->axis3 != event->touchpadVertical)) + { + entry->eventType = BUTTON_EVENT_AXIS_CHANGED; + entry->axis1Id = 2; + entry->axis1Val = event->touchpadHorizontal; + entry->axis2Id = 3; + entry->axis2Val = event->touchpadVertical; + incrementAndPostButtonQueue(so->ctx); + entry = &(so->ctx->buttonQueue.entry[so->ctx->buttonQueue.nextWriteIndex]); + + } + } + + if (event->pressedButtonsValid) + { + so->buttonmask = event->pressedButtons; + } + if (event->batteryChargeValid) + { + so->charge = event->batteryCharge; + } + if (event->touchpadHorizontalValid) + { + so->axis2 = event->touchpadHorizontal; + } + if (event->touchpadVerticalValid) + { + so->axis3 = event->touchpadVertical; + } + if (event->triggerHighResValid) + { + so->axis1 = event->triggerHighRes; + } +} + +uint8_t isPopulated; //probably can remove this given the semaphore in the parent struct. helps with debugging +uint8_t eventType; +uint8_t buttonId; +uint8_t axis1Id; +uint16_t axis1Val; +uint8_t axis2Id; +uint16_t axis2Val; +SurviveObject *so; + static void handle_watchman( SurviveObject * w, uint8_t * readdata ) { @@ -885,6 +1041,8 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) uint8_t qty = POP1; uint8_t time2 = POP1; uint8_t type = POP1; + + qty-=2; int propset = 0; int doimu = 0; @@ -899,6 +1057,8 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) { qty-=1; w->buttonmask = POP1; + + printf("buttonmask is %d\n", w->buttonmask); type &= ~0x01; } if( type & 0x04 ) @@ -1237,7 +1397,7 @@ void survive_data_cb( SurviveUSBInterface * si ) } else if( id == 38 ) { - w->ison = 0; + w->ison = 0; // turning off } else { @@ -1304,32 +1464,53 @@ void survive_data_cb( SurviveUSBInterface * si ) //0x3E uint8 1 someBitFieldMaybe 0x00 : ping / 0x64 : user input //0x3F ? 1 ? unknown - typedef struct - { - //uint8_t reportId; - uint16_t reportType; - uint32_t reportCount; - uint32_t pressedButtons; - uint16_t triggerOrBattery; - uint16_t batteryCharge; - uint32_t hardwareId; - int16_t touchpadHorizontal; - int16_t touchpadVertical; - uint16_t unknown1; - uint16_t triggerHighRes; - uint8_t unknown2; - uint8_t unknown3; - uint8_t unknown4; - uint16_t triggerRaw; - uint8_t unknown5; - uint8_t unknown6; // maybe some bitfield? - uint8_t unknown7; - } usb_buttons_raw; - - usb_buttons_raw *raw = (usb_buttons_raw*) readdata; - if (raw->reportType == 0x100) + //typedef struct + //{ + // //uint8_t reportId; + // uint16_t reportType; + // uint32_t reportCount; + // uint32_t pressedButtons; + // uint16_t triggerOrBattery; + // uint8_t batteryCharge; + // uint32_t hardwareId; + // int16_t touchpadHorizontal; + // int16_t touchpadVertical; + // uint16_t unknown1; + // uint16_t triggerHighRes; + // uint8_t unknown2; + // uint8_t unknown3; + // uint8_t unknown4; + // uint16_t triggerRaw; + // uint8_t unknown5; + // uint8_t unknown6; // maybe some bitfield? + // uint8_t unknown7; + //} usb_buttons_raw; + + //usb_buttons_raw *raw = (usb_buttons_raw*) readdata; + if (*((uint16_t*)(&(readdata[0x0]))) == 0x100) { - printf("Buttons: %8.8x\n", raw->pressedButtons); + buttonEvent bEvent; + memset(&bEvent, 0, sizeof(bEvent)); + + bEvent.pressedButtonsValid = 1; + bEvent.pressedButtons = *((uint32_t*)(&(readdata[0x07]))); + bEvent.triggerHighResValid = 1; + //bEvent.triggerHighRes = raw->triggerHighRes; + //bEvent.triggerHighRes = (raw->pressedButtons & 0xff000000) >> 24; // this seems to provide the same data at 2x the resolution as above + //bEvent.triggerHighRes = raw->triggerRaw; + + bEvent.triggerHighRes = *((uint16_t*)(&(readdata[0x19]))); + bEvent.touchpadHorizontalValid = 1; + //bEvent.touchpadHorizontal = raw->touchpadHorizontal; + bEvent.touchpadHorizontal = *((int16_t*)(&(readdata[0x13]))); + bEvent.touchpadVerticalValid = 1; + //bEvent.touchpadVertical = raw->touchpadVertical; + bEvent.touchpadVertical = *((int16_t*)(&(readdata[0x15]))); + + //printf("%4.4x\n", bEvent.triggerHighRes); + registerButtonEvent(obj, &bEvent); + + //printf("Buttons: %8.8x\n", raw->pressedButtons); } int a = 0; } -- cgit v1.3.1 From f183aa480c549695ac5b481fade04e62f71d1e0a Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Wed, 3 Jan 2018 18:58:43 -0700 Subject: Controller Buttons Fully Implemented Fully plumbed support for controller buttons Also, commented haptic call because it messed with the vive_magic calls, given where I had it. --- include/libsurvive/survive.h | 8 ++++---- include/libsurvive/survive_types.h | 8 +++++++- src/survive.c | 36 +++++++++++++++++++++++++++++------- src/survive_process.c | 11 +++++++++++ src/survive_vive.c | 28 ++++++++++++++-------------- 5 files changed, 65 insertions(+), 26 deletions(-) (limited to 'include') diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index f3a17c8..e4afadf 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -93,10 +93,7 @@ struct config_group; #define BUTTON_QUEUE_MAX_LEN 32 -#define BUTTON_EVENT_BUTTON_NONE 0 -#define BUTTON_EVENT_BUTTON_DOWN 1 -#define BUTTON_EVENT_BUTTON_UP 2 -#define BUTTON_EVENT_AXIS_CHANGED 3 + // note: buttonId and axisId are 1-indexed values. // a value of 0 for an id means that no data is present in that value @@ -129,6 +126,7 @@ struct SurviveContext light_process_func lightproc; imu_process_func imuproc; angle_process_func angleproc; + button_process_func buttonproc; struct config_group* global_config_values; struct config_group* lh_config; //lighthouse configs @@ -163,6 +161,7 @@ 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_install_button_fn( SurviveContext * ctx, button_process_func fbp ); void survive_close( SurviveContext * ctx ); int survive_poll( SurviveContext * ctx ); @@ -185,6 +184,7 @@ int survive_cal_get_status( struct SurviveContext * ctx, char * description, int void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length , uint32_t lh); 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, uint32_t lh ); +void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val); ////////////////////// Survive Drivers //////////////////////////// diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h index 224719e..fa3eb2f 100644 --- a/include/libsurvive/survive_types.h +++ b/include/libsurvive/survive_types.h @@ -27,6 +27,12 @@ typedef struct SurvivePose #define INTBUFFSIZE 64 #define SENSORS_PER_OBJECT 32 +// These are used for the eventType of button_process_func +#define BUTTON_EVENT_BUTTON_NONE 0 +#define BUTTON_EVENT_BUTTON_DOWN 1 +#define BUTTON_EVENT_BUTTON_UP 2 +#define BUTTON_EVENT_AXIS_CHANGED 3 + typedef struct SurviveObject SurviveObject; typedef struct SurviveContext SurviveContext; typedef struct BaseStationData BaseStationData; @@ -36,7 +42,7 @@ 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, uint32_t lighthouse); 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, uint32_t lh); - +typedef void (*button_process_func)(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val); //Device drivers (prefix your drivers with "DriverReg") i.e. // REGISTER_LINKTIME( DriverRegHTCVive ); diff --git a/src/survive.c b/src/survive.c index 99b6648..d3c0918 100755 --- a/src/survive.c +++ b/src/survive.c @@ -64,13 +64,25 @@ static void button_servicer(void * context) return; } - printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n", - entry->eventType, - entry->buttonId, - entry->axis1Id, - entry->axis1Val, - entry->axis2Id, - entry->axis2Val); + //printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n", + // entry->eventType, + // entry->buttonId, + // entry->axis1Id, + // entry->axis1Val, + // entry->axis2Id, + // entry->axis2Val); + + button_process_func butt_func = ctx->buttonproc; + if (butt_func) + { + butt_func(entry->so, + entry->eventType, + entry->buttonId, + entry->axis1Id, + entry->axis1Val, + entry->axis2Id, + entry->axis2Val); + } ctx->buttonQueue.nextReadIndex++; if (ctx->buttonQueue.nextReadIndex >= BUTTON_QUEUE_MAX_LEN) @@ -171,6 +183,7 @@ SurviveContext * survive_init( int headless ) // start the thread to process button data ctx->buttonservicethread = OGCreateThread(button_servicer, ctx); + survive_install_button_fn(ctx, NULL); return ctx; } @@ -216,6 +229,15 @@ void survive_install_angle_fn( SurviveContext * ctx, angle_process_func fbp ) ctx->angleproc = survive_default_angle_process; } +void survive_install_button_fn(SurviveContext * ctx, button_process_func fbp) +{ + if (fbp) + ctx->buttonproc = fbp; + else + ctx->buttonproc = survive_default_button_process; + +} + int survive_add_object( SurviveContext * ctx, SurviveObject * obj ) { int oldct = ctx->objs_ct; diff --git a/src/survive_process.c b/src/survive_process.c index 3af2da9..eaed881 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -63,6 +63,17 @@ void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode } } +void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val) +{ + // do nothing. + printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n", + eventType, + buttonId, + axis1Id, + axis1Val, + axis2Id, + axis2Val); +} void survive_default_imu_process( SurviveObject * so, int mask, FLT * accelgyromag, uint32_t timecode, int id ) { diff --git a/src/survive_vive.c b/src/survive_vive.c index e271e3e..3df60ba 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -619,30 +619,30 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo //#endif -//#if 0 - for (int i = 0; i < 0xff; i++) +#if 0 + for (int i = 0; i < 0xf; i++) { //uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x7, 0 /*right*/, 0xFF /*period on*/, 0xFF/*period on*/, 0xFF/*period off*/, 0xFF/*period off*/, 0xFF /* repeat Count */, 0xFF /* repeat count */ }; //uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x07, 0x00, 0xf4, 0x01, 0xb5, 0xa2, 0x01, 0x00 }; // data taken from Nairol's captures uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x07, 0x00, 0xf4, 0x01, 0xb5, 0xa2, 0x01, 0x00 }; - //r = update_feature_report( sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) ); - r = update_feature_report(sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof(vive_controller_haptic_pulse)); + r = update_feature_report( sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) ); + r = getupdate_feature_report(sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof(vive_controller_haptic_pulse)); SV_INFO("UCR: %d", r); if (r != sizeof(vive_controller_haptic_pulse)) printf("HAPTIC FAILED **************************\n"); // return 5; OGUSleep(1000); } -//#endif +#endif #if 0 - // working code to turn off a wireless controller: - { - uint8_t vive_controller_off[64] = { 0xff, 0x9f, 0x04, 'o', 'f', 'f', '!' }; - //r = update_feature_report( sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) ); - r = update_feature_report(sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_off, sizeof(vive_controller_off)); - SV_INFO("UCR: %d", r); - if (r != sizeof(vive_controller_off)) printf("OFF FAILED **************************\n"); // return 5; - OGUSleep(1000); - } + //// working code to turn off a wireless controller: + //{ + // uint8_t vive_controller_off[64] = { 0xff, 0x9f, 0x04, 'o', 'f', 'f', '!' }; + // //r = update_feature_report( sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) ); + // r = update_feature_report(sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_off, sizeof(vive_controller_off)); + // SV_INFO("UCR: %d", r); + // if (r != sizeof(vive_controller_off)) printf("OFF FAILED **************************\n"); // return 5; + // OGUSleep(1000); + //} #endif //if (sv->udev[USB_DEV_TRACKER0]) //{ -- cgit v1.3.1 From 499b80ae7b538f8e66f5ec8bfa60c7136a3babf5 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Thu, 4 Jan 2018 08:47:42 -0700 Subject: Haptic Call Plumbed The plumbing is now in place for the haptic call. Left in place a "demo" where haptic is called when a controller's trigger is pulled --- include/libsurvive/survive.h | 5 +++ include/libsurvive/survive_types.h | 2 + src/survive.c | 13 +++++- src/survive_process.c | 26 +++++++++--- src/survive_vive.c | 87 +++++++++++++++++++++++++++++++++----- 5 files changed, 115 insertions(+), 18 deletions(-) (limited to 'include') diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index e4afadf..0cfab1f 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -19,6 +19,7 @@ struct SurviveObject char codename[4]; //3 letters, null-terminated. Currently HMD, WM0, WM1. char drivername[4]; //3 letters for driver. Currently "HTC" + void *driver; int32_t buttonmask; int16_t axis1; @@ -68,6 +69,7 @@ struct SurviveObject FLT* gyro_bias; // size is FLT*3. contains x,y,z FLT* gyro_scale; // size is FLT*3. contains x,y,z + haptic_func haptic; //Debug int tsl; @@ -179,6 +181,9 @@ void survive_cal_install( SurviveContext * ctx ); //XXX This will be removed if // Read back a human-readable string description of the calibration status int survive_cal_get_status( struct SurviveContext * ctx, char * description, int description_length ); +// Induce haptic feedback +int survive_haptic(SurviveObject * so, uint8_t reserved, uint16_t pulseHigh, uint16_t pulseLow, uint16_t repeatCount); + //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 , uint32_t lh); diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h index fa3eb2f..9a6e148 100644 --- a/include/libsurvive/survive_types.h +++ b/include/libsurvive/survive_types.h @@ -44,6 +44,8 @@ typedef void (*imu_process_func)( SurviveObject * so, int mask, FLT * accelgyro, typedef void (*angle_process_func)( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh); typedef void (*button_process_func)(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val); +typedef int(*haptic_func)(SurviveObject * so, uint8_t reserved, uint16_t pulseHigh , uint16_t pulseLow, uint16_t repeatCount); + //Device drivers (prefix your drivers with "DriverReg") i.e. // REGISTER_LINKTIME( DriverRegHTCVive ); typedef int (*DeviceDriver)( SurviveContext * ctx ); diff --git a/src/survive.c b/src/survive.c index d3c0918..75c07bf 100755 --- a/src/survive.c +++ b/src/survive.c @@ -60,7 +60,7 @@ static void button_servicer(void * context) // should never happen. indicates failure of code pushing stuff onto // the buttonQueue // if it does happen, it will kill all future button input - printf("ERROR: Unpopulated ButtonQueueEntry!"); + printf("ERROR: Unpopulated ButtonQueueEntry! NextReadIndex=%d\n", ctx->buttonQueue.nextReadIndex); return; } @@ -272,6 +272,17 @@ int survive_send_magic( SurviveContext * ctx, int magic_code, void * data, int d return 0; } +int survive_haptic(SurviveObject * so, uint8_t reserved, uint16_t pulseHigh, uint16_t pulseLow, uint16_t repeatCount) +{ + if (NULL == so || NULL == so->haptic) + { + return -404; + } + + return so->haptic(so, reserved, pulseHigh, pulseLow, repeatCount); +} + + void survive_close( SurviveContext * ctx ) { const char * DriverName; diff --git a/src/survive_process.c b/src/survive_process.c index eaed881..4b86144 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -66,13 +66,25 @@ void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val) { // do nothing. - printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n", - eventType, - buttonId, - axis1Id, - axis1Val, - axis2Id, - axis2Val); + //printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n", + // eventType, + // buttonId, + // axis1Id, + // axis1Val, + // axis2Id, + // axis2Val); + if (buttonId == 24 && eventType == 1) // trigger engage + { + for (int j = 0; j < 40; j++) + { + for (int i = 0; i < 0x1; i++) + { + survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100); + OGUSleep(5000); + } + OGUSleep(20000); + } + } } void survive_default_imu_process( SurviveObject * so, int mask, FLT * accelgyromag, uint32_t timecode, int id ) diff --git a/src/survive_vive.c b/src/survive_vive.c index 3df60ba..9e4a8b8 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -620,17 +620,24 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo //#endif #if 0 - for (int i = 0; i < 0xf; i++) + for (int j=0; j < 40; j++) { - //uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x7, 0 /*right*/, 0xFF /*period on*/, 0xFF/*period on*/, 0xFF/*period off*/, 0xFF/*period off*/, 0xFF /* repeat Count */, 0xFF /* repeat count */ }; - //uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x07, 0x00, 0xf4, 0x01, 0xb5, 0xa2, 0x01, 0x00 }; // data taken from Nairol's captures - uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x07, 0x00, 0xf4, 0x01, 0xb5, 0xa2, 0x01, 0x00 }; - r = update_feature_report( sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) ); - r = getupdate_feature_report(sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof(vive_controller_haptic_pulse)); - SV_INFO("UCR: %d", r); - if (r != sizeof(vive_controller_haptic_pulse)) printf("HAPTIC FAILED **************************\n"); // return 5; - OGUSleep(1000); + for (int i = 0; i < 0x1; i++) + { + //uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x7, 0 /*right*/, 0xFF /*period on*/, 0xFF/*period on*/, 0xFF/*period off*/, 0xFF/*period off*/, 0xFF /* repeat Count */, 0xFF /* repeat count */ }; + //uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x07, 0x00, 0xf4, 0x01, 0xb5, 0xa2, 0x01, 0x00 }; // data taken from Nairol's captures + uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x07, 0x00, 0xf4, 0x01, 0xb5, 0xa2, 0x01* j, 0x00 }; + r = update_feature_report( sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) ); + r = getupdate_feature_report(sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof(vive_controller_haptic_pulse)); + SV_INFO("UCR: %d", r); + if (r != sizeof(vive_controller_haptic_pulse)) printf("HAPTIC FAILED **************************\n"); // return 5; + OGUSleep(5000); + } + + OGUSleep(20000); } + + #endif #if 0 @@ -686,6 +693,54 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo return 0; } +int survive_vive_send_haptic(SurviveObject * so, uint8_t reserved, uint16_t pulseHigh, uint16_t pulseLow, uint16_t repeatCount) +{ + SurviveViveData *sv = (SurviveViveData*)so->driver; + + if (NULL == sv) + { + return -500; + } + + int r; + uint8_t vive_controller_haptic_pulse[64] = { + 0xff, 0x8f, 0x07, 0x00, + pulseHigh & 0xff00 >> 8, pulseHigh & 0xff, + pulseLow & 0xff00 >> 8, pulseLow & 0xff, + repeatCount & 0xff00 >> 8, repeatCount & 0xff, + }; + + r = update_feature_report(sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof(vive_controller_haptic_pulse)); + r = getupdate_feature_report(sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof(vive_controller_haptic_pulse)); + //SV_INFO("UCR: %d", r); + if (r != sizeof(vive_controller_haptic_pulse)) + { + printf("HAPTIC FAILED **************************\n"); + return -1; + } + + return 0; + + //for (int j = 0; j < 40; j++) + //{ + // for (int i = 0; i < 0x1; i++) + // { + // //uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x7, 0 /*right*/, 0xFF /*period on*/, 0xFF/*period on*/, 0xFF/*period off*/, 0xFF/*period off*/, 0xFF /* repeat Count */, 0xFF /* repeat count */ }; + // //uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x07, 0x00, 0xf4, 0x01, 0xb5, 0xa2, 0x01, 0x00 }; // data taken from Nairol's captures + // uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x07, 0x00, 0xf4, 0x01, 0xb5, 0xa2, 0x01 * j, 0x00 }; + // r = update_feature_report(sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof(vive_controller_haptic_pulse)); + // r = getupdate_feature_report(sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof(vive_controller_haptic_pulse)); + // if (r != sizeof(vive_controller_haptic_pulse)) printf("HAPTIC FAILED **************************\n"); // return 5; + // OGUSleep(5000); + // } + + // OGUSleep(20000); + //} + + ////OGUSleep(5000); + //return 0; +} + void survive_vive_usb_close( SurviveViveData * sv ) { int i; @@ -903,9 +958,10 @@ void incrementAndPostButtonQueue(SurviveContext *ctx) { ButtonQueueEntry *entry = &(ctx->buttonQueue.entry[ctx->buttonQueue.nextWriteIndex]); - if (OGGetSema(ctx->buttonQueue.buttonservicesem) >= BUTTON_QUEUE_MAX_LEN) + if (OGGetSema(ctx->buttonQueue.buttonservicesem) >= BUTTON_QUEUE_MAX_LEN-1) { // There's not enough space to write this entry. Clear it out and move along + //printf("Button Buffer Full\n"); memset(entry, 0, sizeof(ButtonQueueEntry)); return; } @@ -1759,6 +1815,9 @@ void init_SurviveObject(SurviveObject* so) { so->acc_bias = NULL; so->gyro_scale = NULL; so->gyro_bias = NULL; + so->haptic = NULL; + so->PoserData = NULL; + so->disambiguator_data = NULL; } int DriverRegHTCVive( SurviveContext * ctx ) @@ -1789,18 +1848,23 @@ int DriverRegHTCVive( SurviveContext * ctx ) hmd->ctx = ctx; + hmd->driver = sv; memcpy( hmd->codename, "HMD", 4 ); memcpy( hmd->drivername, "HTC", 4 ); wm0->ctx = ctx; + wm0->driver = sv; memcpy( wm0->codename, "WM0", 4 ); memcpy( wm0->drivername, "HTC", 4 ); wm1->ctx = ctx; + wm1->driver = sv; memcpy( wm1->codename, "WM1", 4 ); memcpy( wm1->drivername, "HTC", 4 ); tr0->ctx = ctx; + tr0->driver = sv; memcpy( tr0->codename, "TR0", 4 ); memcpy( tr0->drivername, "HTC", 4 ); ww0->ctx = ctx; + ww0->driver = sv; memcpy( ww0->codename, "WW0", 4 ); memcpy( ww0->drivername, "HTC", 4 ); @@ -1844,6 +1908,9 @@ int DriverRegHTCVive( SurviveContext * ctx ) wm1->timecenter_ticks = wm1->timebase_hz / 240; tr0->timecenter_ticks = tr0->timebase_hz / 240; ww0->timecenter_ticks = ww0->timebase_hz / 240; + + wm0->haptic = survive_vive_send_haptic; + wm1->haptic = survive_vive_send_haptic; /* int i; int locs = hmd->nr_locations; -- cgit v1.3.1 From 735a8bd11070b0c563e891ff8b70ce297a52a367 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Fri, 5 Jan 2018 03:54:29 -0700 Subject: Add standard output mechanism for posers Added a raw pose output/ callback that the posers can call when they have calculated a pose. --- include/libsurvive/survive.h | 5 ++++- include/libsurvive/survive_types.h | 3 ++- src/poser_turveytori.c | 8 ++++++-- src/survive.c | 9 ++++++++- src/survive_data.c | 4 ++-- src/survive_process.c | 39 +++++++++++++++++++++++++++++--------- 6 files changed, 52 insertions(+), 16 deletions(-) (limited to 'include') diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 0cfab1f..4821e63 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -129,6 +129,7 @@ struct SurviveContext imu_process_func imuproc; angle_process_func angleproc; button_process_func buttonproc; + raw_pose_func rawposeproc; struct config_group* global_config_values; struct config_group* lh_config; //lighthouse configs @@ -163,7 +164,8 @@ 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_install_button_fn( SurviveContext * ctx, button_process_func fbp ); +void survive_install_button_fn(SurviveContext * ctx, button_process_func fbp); +void survive_install_raw_pose_fn(SurviveContext * ctx, raw_pose_func fbp); void survive_close( SurviveContext * ctx ); int survive_poll( SurviveContext * ctx ); @@ -190,6 +192,7 @@ void survive_default_light_process( SurviveObject * so, int sensor_id, int acode 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, uint32_t lh ); void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val); +void survive_default_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *position, FLT *quaternion); ////////////////////// Survive Drivers //////////////////////////// diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h index 9a6e148..5384345 100644 --- a/include/libsurvive/survive_types.h +++ b/include/libsurvive/survive_types.h @@ -42,7 +42,8 @@ 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, uint32_t lighthouse); 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, uint32_t lh); -typedef void (*button_process_func)(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val); +typedef void(*button_process_func)(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val); +typedef void(*raw_pose_func)(SurviveObject * so, uint8_t lighthouse, FLT *position, FLT *quaternion); typedef int(*haptic_func)(SurviveObject * so, uint8_t reserved, uint16_t pulseHigh , uint16_t pulseLow, uint16_t repeatCount); diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index ac979c7..28e533e 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1177,7 +1177,7 @@ static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *in } - printf("Ri=%3d Fitness=%3f ", i, lastMatchFitness); + if (ttDebug) printf("Ri=%3d Fitness=%3f ", i, lastMatchFitness); } @@ -1638,7 +1638,11 @@ static void QuickPose(SurviveObject *so, int lh) SolveForLighthouse(pos, quat, to, so, 0, lh, 0); - printf("P&O: [% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]); + //printf("P&O: [% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]); + if (so->ctx->rawposeproc) + { + so->ctx->rawposeproc(so, lh, pos, quat); + } if (ttDebug) printf("!\n"); } diff --git a/src/survive.c b/src/survive.c index 75c07bf..1c406e8 100755 --- a/src/survive.c +++ b/src/survive.c @@ -184,6 +184,7 @@ SurviveContext * survive_init( int headless ) // start the thread to process button data ctx->buttonservicethread = OGCreateThread(button_servicer, ctx); survive_install_button_fn(ctx, NULL); + survive_install_raw_pose_fn(ctx, NULL); return ctx; } @@ -235,9 +236,15 @@ void survive_install_button_fn(SurviveContext * ctx, button_process_func fbp) ctx->buttonproc = fbp; else ctx->buttonproc = survive_default_button_process; - } +void survive_install_raw_pose_fn(SurviveContext * ctx, raw_pose_func fbp) +{ + if (fbp) + ctx->rawposeproc = fbp; + else + ctx->rawposeproc = survive_default_raw_pose_process; +} int survive_add_object( SurviveContext * ctx, SurviveObject * obj ) { int oldct = ctx->objs_ct; diff --git a/src/survive_data.c b/src/survive_data.c index 1b80269..0427659 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -419,8 +419,8 @@ void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le ) } if (lcd->per_sweep.activeLighthouse < 0) { - fprintf(stderr, "WARNING: No active lighthouse!\n"); - fprintf(stderr, " %2d %8d %d %d\n", le->sensor_id, le->length,lcd->per_sweep.lh_acode[0],lcd->per_sweep.lh_acode[1]); + //fprintf(stderr, "WARNING: No active lighthouse!\n"); + //fprintf(stderr, " %2d %8d %d %d\n", le->sensor_id, le->length,lcd->per_sweep.lh_acode[0],lcd->per_sweep.lh_acode[1]); return; } diff --git a/src/survive_process.c b/src/survive_process.c index 4b86144..42d2897 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -66,20 +66,34 @@ void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val) { // do nothing. - //printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n", - // eventType, - // buttonId, - // axis1Id, - // axis1Val, - // axis2Id, - // axis2Val); + printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n", + eventType, + buttonId, + axis1Id, + axis1Val, + axis2Id, + axis2Val); if (buttonId == 24 && eventType == 1) // trigger engage { - for (int j = 0; j < 40; j++) + for (int j = 0; j < 6; j++) { - for (int i = 0; i < 0x1; i++) + for (int i = 0; i < 0x5; i++) { survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100); + //survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100); + OGUSleep(1000); + } + OGUSleep(20000); + } + } + if (buttonId == 2 && eventType == 1) // trigger engage + { + for (int j = 0; j < 6; j++) + { + for (int i = 0; i < 0x1; i++) + { + survive_haptic(so, 0, 0xf401, 0x05a2, 0xf100); + //survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100); OGUSleep(5000); } OGUSleep(20000); @@ -87,6 +101,13 @@ void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8 } } +void survive_default_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *pos, FLT *quat) +{ + // print the pose; + printf("Pose: [%2.2x][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]); + +} + void survive_default_imu_process( SurviveObject * so, int mask, FLT * accelgyromag, uint32_t timecode, int id ) { if( so->PoserFn ) -- cgit v1.3.1 From 7817da63526f35d10d6d4f6b3d9c02280719e023 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Mon, 15 Jan 2018 21:50:10 -0700 Subject: Fix compiler warnings --- include/libsurvive/survive.h | 5 ++--- redist/json_helpers.c | 2 +- src/poser_octavioradii.c | 2 +- src/poser_turveytori.c | 5 ++--- src/survive.c | 10 ++++++---- src/survive_config.c | 2 +- 6 files changed, 13 insertions(+), 13 deletions(-) (limited to 'include') diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 4821e63..3d0d472 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -4,7 +4,6 @@ #include #include "survive_types.h" #include "poser.h" -#include "os_generic.h" #ifdef __cplusplus extern "C" { @@ -117,7 +116,7 @@ typedef struct { uint8_t nextReadIndex; //init to 0 uint8_t nextWriteIndex; // init to 0 - og_sema_t buttonservicesem; + void* buttonservicesem; ButtonQueueEntry entry[BUTTON_QUEUE_MAX_LEN]; } ButtonQueue; @@ -150,7 +149,7 @@ struct SurviveContext uint8_t isClosing; // flag to indicate if threads should terminate themselves - og_thread_t buttonservicethread; + void* buttonservicethread; ButtonQueue buttonQueue; }; diff --git a/redist/json_helpers.c b/redist/json_helpers.c index af7ddda..8da841b 100644 --- a/redist/json_helpers.c +++ b/redist/json_helpers.c @@ -121,7 +121,7 @@ char* load_file_to_mem(const char* path) { fseek( f, 0, SEEK_SET ); char * JSON_STRING = malloc( len + 1); memset(JSON_STRING,0,len+1); - int i = fread( JSON_STRING, len, 1, f ); i = i; //Ignore return value. + size_t i = fread( JSON_STRING, len, 1, f ); i = i; //Ignore return value. fclose( f ); return JSON_STRING; } diff --git a/src/poser_octavioradii.c b/src/poser_octavioradii.c index be1e9f7..5805e1a 100644 --- a/src/poser_octavioradii.c +++ b/src/poser_octavioradii.c @@ -591,7 +591,7 @@ int PoserOctavioRadii( SurviveObject * so, PoserData * pd ) } else { - dd->hitCount[i][l->lh][axis] *= 0.5; + dd->hitCount[i][l->lh][axis] = (int)((double)dd->hitCount[i][l->lh][axis] * 0.5); } } //if (0 == l->lh) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 59ff25e..de1516a 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1,5 +1,5 @@ #include -#include +#include "survive_config.h" #include #include #include @@ -785,7 +785,6 @@ FLT RotationEstimateFitnessQuaternion(Point lhPoint, FLT *quaternion, TrackedObj FLT throwaway = RotationEstimateFitnessAxisAngle(lhPoint, axisAngle, obj); - int a = throwaway; return throwaway; } @@ -1397,7 +1396,7 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob SolveForRotationQuat(rotQuat, obj, refinedEstimateGd); SolveForRotation(rot, obj, refinedEstimateGd); FLT objPos[3]; - FLT objPos2[3]; + //FLT objPos2[3]; //{ // toriData->lastLhRotQuat[lh][0] = rotQuat[0]; diff --git a/src/survive.c b/src/survive.c index a5ca68f..0386275 100755 --- a/src/survive.c +++ b/src/survive.c @@ -8,6 +8,7 @@ #include #include "survive_config.h" +#include "os_generic.h" #ifdef __APPLE__ #define z_const const @@ -40,7 +41,7 @@ static void survivenote( struct SurviveContext * ctx, const char * fault ) fprintf( stderr, "Info: %s\n", fault ); } -static void button_servicer(void * context) +static void *button_servicer(void * context) { SurviveContext *ctx = (SurviveContext*)context; @@ -51,7 +52,7 @@ static void button_servicer(void * context) if (ctx->isClosing) { // we're shutting down. Close. - return; + return NULL; } ButtonQueueEntry *entry = &(ctx->buttonQueue.entry[ctx->buttonQueue.nextReadIndex]); @@ -61,7 +62,7 @@ static void button_servicer(void * context) // the buttonQueue // if it does happen, it will kill all future button input printf("ERROR: Unpopulated ButtonQueueEntry! NextReadIndex=%d\n", ctx->buttonQueue.nextReadIndex); - return; + return NULL; } //printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n", @@ -90,6 +91,7 @@ static void button_servicer(void * context) ctx->buttonQueue.nextReadIndex = 0; } }; + return NULL; } SurviveContext * survive_init( int headless ) @@ -182,7 +184,7 @@ SurviveContext * survive_init( int headless ) ctx->buttonQueue.buttonservicesem = OGCreateSema(); // start the thread to process button data - ctx->buttonservicethread = OGCreateThread(&button_servicer, ctx); + ctx->buttonservicethread = OGCreateThread(button_servicer, ctx); survive_install_button_fn(ctx, NULL); survive_install_raw_pose_fn(ctx, NULL); diff --git a/src/survive_config.c b/src/survive_config.c index 0961651..46c6658 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -187,7 +187,7 @@ uint16_t config_read_float_array(config_group *cg, const char *tag, FLT* values, config_entry *cv = find_config_entry(cg, tag); if (cv != NULL) { - for (int i=0; i < CFG_MIN(count, cv->elements); i++) + for (unsigned int i=0; i < CFG_MIN(count, cv->elements); i++) { values[i] = ((double*)cv->data)[i]; } -- cgit v1.3.1 From c9fec03986e23958e0dee3f0d1749fa8f5769ba7 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Tue, 16 Jan 2018 20:37:56 -0700 Subject: Add user pointer to survive context --- include/libsurvive/survive.h | 2 ++ 1 file changed, 2 insertions(+) (limited to 'include') diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 3d0d472..30f5817 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -152,6 +152,8 @@ struct SurviveContext void* buttonservicethread; ButtonQueue buttonQueue; + void *user_ptr; + }; SurviveContext * survive_init( int headless ); -- cgit v1.3.1 From 728f6f9e7f0a99aa584e7f35ade387e852b5fa83 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Wed, 7 Mar 2018 22:41:10 -0700 Subject: Added a check at init that makes sure the user agrees with what FLT is --- include/libsurvive/survive.h | 10 +++++++++- src/survive.c | 9 ++++++++- 2 files changed, 17 insertions(+), 2 deletions(-) (limited to 'include') diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 30f5817..d9b5f08 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -156,7 +156,15 @@ struct SurviveContext }; -SurviveContext * survive_init( int headless ); +SurviveContext * survive_init_internal( int headless ); + +// Baked in size of FLT to verify users of the library have the correct setting. +void survive_verify_FLT_size(uint32_t user_size); + +static inline SurviveContext * survive_init( int headless ) { + survive_verify_FLT_size(sizeof(FLT)); + return survive_init_internal( 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. diff --git a/src/survive.c b/src/survive.c index 0386275..4e52637 100755 --- a/src/survive.c +++ b/src/survive.c @@ -94,7 +94,14 @@ static void *button_servicer(void * context) return NULL; } -SurviveContext * survive_init( int headless ) +void survive_verify_FLT_size(uint32_t user_size) { + if(sizeof(FLT) != user_size) { + fprintf(stderr, "FLT type incompatible; the shared library has FLT size %lu vs user program %u\n", sizeof(FLT), user_size); + exit(-1); + } +} + +SurviveContext * survive_init_internal( int headless ) { #ifdef RUNTIME_SYMNUM if( !did_runtime_symnum ) -- cgit v1.3.1 From 5aa1b9214745a625ab644846ada3034e7c38f493 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Thu, 8 Mar 2018 15:08:14 -0700 Subject: Added playback device driver --- Makefile | 2 +- data_recorder.c | 118 +++++++----- include/libsurvive/survive_types.h | 6 + src/survive_default_devices.c | 46 +++++ src/survive_default_devices.h | 12 ++ src/survive_playback.c | 374 +++++++++++++++++++++++++++++++++++++ src/survive_vive.c | 101 +++------- 7 files changed, 539 insertions(+), 120 deletions(-) create mode 100644 src/survive_default_devices.c create mode 100644 src/survive_default_devices.h create mode 100755 src/survive_playback.c (limited to 'include') diff --git a/Makefile b/Makefile index 69b8e37..e57dae5 100644 --- a/Makefile +++ b/Makefile @@ -37,7 +37,7 @@ REDISTS:=redist/json_helpers.o redist/linmath.o redist/jsmn.o redist/os_generic. ifeq ($(UNAME), Darwin) REDISTS:=$(REDISTS) redist/hid-osx.c endif -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 src/survive_cal.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_default_devices.o src/survive_vive.o src/survive_playback.o src/survive_config.o src/survive_cal.o #If you want to use HIDAPI on Linux. diff --git a/data_recorder.c b/data_recorder.c index 002357e..323d208 100644 --- a/data_recorder.c +++ b/data_recorder.c @@ -3,6 +3,7 @@ #ifdef __linux__ #include #endif + #include #include #include @@ -10,9 +11,14 @@ #include #include #include +#include +#include +#include struct SurviveContext * ctx; +FILE* output_file = 0; + void HandleKey( int keycode, int bDown ) { if( !bDown ) return; @@ -43,56 +49,70 @@ int bufferpts[32*2*3]; char buffermts[32*128*3]; int buffertimeto[32*3]; +uint64_t timestamp_in_us() { + static uint64_t start_time_us = 0; + struct timeval tv; + gettimeofday(&tv,NULL); + uint64_t now = (uint64_t)tv.tv_sec * 1000000L + tv.tv_usec; + if(start_time_us == 0) + start_time_us = now; + return now - start_time_us; +} + +int write_to_output(const char *format, ...) +{ + va_list args; + va_start(args, format); + fprintf(output_file, "%lu ", timestamp_in_us()); + vfprintf(output_file, format, args); + + va_end(args); +} + void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh) { survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length, lh); - if( acode == -1 ) return; -//return; + if( acode == -1 ) { + write_to_output( "A %s %d %d %d %u %u %u\n", so->codename, sensor_id, acode, timeinsweep, timecode, length, lh ); + return; + } + int jumpoffset = sensor_id; if( strcmp( so->codename, "WM0" ) == 0 ) jumpoffset += 32; else if( strcmp( so->codename, "WM1" ) == 0 ) jumpoffset += 64; - - if( acode == 0 || acode == 2 ) //data = 0 - { - printf( "L X %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length ); - bufferpts[jumpoffset*2+0] = (timeinsweep-100000)/500; - buffertimeto[jumpoffset] = 0; - } - if( acode == 1 || acode == 3 ) //data = 1 - { - printf( "L Y %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length ); - bufferpts[jumpoffset*2+1] = (timeinsweep-100000)/500; - buffertimeto[jumpoffset] = 0; - } - - - if( acode == 4 || acode == 6 ) //data = 0 - { - printf( "R X %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length ); - bufferpts[jumpoffset*2+0] = (timeinsweep-100000)/500; - buffertimeto[jumpoffset] = 0; - } - if( acode == 5 || acode == 7 ) //data = 1 - { - printf( "R Y %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length ); - bufferpts[jumpoffset*2+1] = (timeinsweep-100000)/500; - buffertimeto[jumpoffset] = 0; - } + const char* LH_ID = 0; + const char* LH_Axis = 0; + + switch(acode) { + case 0: + case 2: + bufferpts[jumpoffset*2+0] = (timeinsweep-100000)/500; + LH_ID = "L"; LH_Axis = "X"; break; + case 1: + case 3: + bufferpts[jumpoffset*2+1] = (timeinsweep-100000)/500; + LH_ID = "L"; LH_Axis = "Y"; break; + case 4: + case 6: + bufferpts[jumpoffset*2+0] = (timeinsweep-100000)/500; + LH_ID = "R"; LH_Axis = "X"; break; + case 5: + case 7: + bufferpts[jumpoffset*2+1] = (timeinsweep-100000)/500; + LH_ID = "R"; LH_Axis = "Y"; break; + } + + write_to_output( "%s %s %s %u %d %d %d %u %u\n", LH_ID, LH_Axis, so->codename, timecode, sensor_id, acode, timeinsweep, length, lh ); + buffertimeto[jumpoffset] = 0; } void my_imu_process( struct SurviveObject * so, int mask, FLT * accelgyro, uint32_t timecode, int id ) { survive_default_imu_process( so, mask, accelgyro, timecode, id ); - -//return; - //if( so->codename[0] == 'H' ) - if( 1 ) - { - 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 ); - } + write_to_output( "I %s %d %u %.17g %.17g %.17g %.17g %.17g %.17g %d\n", so->codename, mask, timecode, accelgyro[0], accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id ); } @@ -156,22 +176,30 @@ void *SurviveThread(void *junk) while(survive_poll(ctx) == 0) { - printf("Do stuff.\n"); - //Do stuff. } return 0; } -int main() +int main(int argc, char** argv) { - // Create the libsurvive thread - OGCreateThread(SurviveThread, 0); + if(argc > 1) { + output_file = fopen(argv[1], "w"); + if(output_file == 0) { + fprintf(stderr, "Could not open %s for writing", argv[1]); + return -1; + } + } else { + output_file = stdout; + } + + // Create the libsurvive thread + OGCreateThread(SurviveThread, 0); - // Wait for the survive thread to load - while (!SurviveThreadLoaded) { OGUSleep(100); } + // Wait for the survive thread to load + while (!SurviveThreadLoaded) { OGUSleep(100); } - // Run the Gui in the main thread - GuiThread(0); + // Run the Gui in the main thread + GuiThread(0); } diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h index 5384345..be1115b 100644 --- a/include/libsurvive/survive_types.h +++ b/include/libsurvive/survive_types.h @@ -14,6 +14,12 @@ extern "C" { #endif #endif +#define float_format "%f" +#define double_format "%lf" +#define _FLT_format2(f) f##_format +#define _FLT_format(f) _FLT_format2(f) +#define FLT_format _FLT_format(FLT) + typedef struct SurvivePose { FLT Pos[3]; diff --git a/src/survive_default_devices.c b/src/survive_default_devices.c new file mode 100644 index 0000000..5d2cda7 --- /dev/null +++ b/src/survive_default_devices.c @@ -0,0 +1,46 @@ +#include +#include +#include "survive_default_devices.h" + +static SurviveObject* survive_create_device(SurviveContext * ctx, + const char* driver_name, + void* driver, + const char* device_name, + haptic_func fn) { + SurviveObject * device = calloc( 1, sizeof( SurviveObject ) ); + + device->ctx = ctx; + device->driver = driver; + memcpy( device->codename, device_name, strlen(device_name) ); + memcpy( device->drivername, driver_name, strlen(driver_name) ); + + device->timebase_hz = 48000000; + device->pulsedist_max_ticks = 500000; + device->pulselength_min_sync = 2200; + device->pulse_in_clear_time = 35000; + device->pulse_max_for_sweep = 1800; + device->pulse_synctime_offset = 20000; + device->pulse_synctime_slack = 5000; + device->timecenter_ticks = device->timebase_hz / 240; + + device->haptic = fn; + + return device; +} + +SurviveObject* survive_create_hmd(SurviveContext * ctx, const char* driver_name, void* driver) { + return survive_create_device(ctx, driver_name, driver, "HMD", 0); +} + +SurviveObject* survive_create_wm0(SurviveContext * ctx, const char* driver_name, void* driver, haptic_func fn) { + return survive_create_device(ctx, driver_name, driver, "WM0", fn); +} +SurviveObject* survive_create_wm1(SurviveContext * ctx, const char* driver_name, void* driver, haptic_func fn) { + return survive_create_device(ctx, driver_name, driver, "WM1", fn); +} +SurviveObject* survive_create_tr0(SurviveContext * ctx, const char* driver_name, void* driver) { + return survive_create_device(ctx, driver_name, driver, "TR0", 0); +} +SurviveObject* survive_create_ww0(SurviveContext * ctx, const char* driver_name, void* driver) { + return survive_create_device(ctx, driver_name, driver, "WW0", 0); +} diff --git a/src/survive_default_devices.h b/src/survive_default_devices.h new file mode 100644 index 0000000..c2ebc0b --- /dev/null +++ b/src/survive_default_devices.h @@ -0,0 +1,12 @@ +#ifndef _SURVIVE_DEFAULT_DEVICES_H +#define _SURVIVE_DEFAULT_DEVICES_H + +#include + +SurviveObject* survive_create_hmd(SurviveContext * ctx, const char* driver_name, void* driver); +SurviveObject* survive_create_wm0(SurviveContext * ctx, const char* driver_name, void* driver, haptic_func cb); +SurviveObject* survive_create_wm1(SurviveContext * ctx, const char* driver_name, void* driver, haptic_func cb); +SurviveObject* survive_create_tr0(SurviveContext * ctx, const char* driver_name, void* driver); +SurviveObject* survive_create_ww0(SurviveContext * ctx, const char* driver_name, void* driver); + +#endif diff --git a/src/survive_playback.c b/src/survive_playback.c new file mode 100755 index 0000000..bb66f25 --- /dev/null +++ b/src/survive_playback.c @@ -0,0 +1,374 @@ +//Unofficial driver for the official Valve/HTC Vive hardware. +// +//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. + +#include +#include +#include +#include +#include +#include +#include +#include +#if !defined(__FreeBSD__) && !defined(__APPLE__) +#include // for alloca +#endif +#include +#include "json_helpers.h" +#include "survive_config.h" +#include "survive_default_devices.h" + +static int jsoneq(const char *json, jsmntok_t *tok, const char *s) { + if (tok->type == JSMN_STRING && (int) strlen(s) == tok->end - tok->start && + strncmp(json + tok->start, s, tok->end - tok->start) == 0) { + return 0; + } + return -1; +} + + +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; + jsmntok_t * tk; + + so->nr_locations = 0; + *floats_out = malloc( sizeof( **floats_out ) * 32 * 3 ); + + for( k = 0; k < pts; k++ ) + { + tk = &t[i+2+k*4]; + + int m; + for( m = 0; m < 3; m++ ) + { + char ctt[128]; + + tk++; + int elemlen = tk->end - tk->start; + + if( tk->type != 4 || elemlen > sizeof( ctt )-1 ) + { + SV_ERROR( "Parse error in JSON\n" ); + return 1; + } + + memcpy( ctt, ct0conf + tk->start, elemlen ); + ctt[elemlen] = 0; + FLT f = atof( ctt ); + int id = so->nr_locations*3+m; + (*floats_out)[id] = f; + } + so->nr_locations++; + } + return 0; +} + +struct SurvivePlaybackData { + SurviveContext * ctx; + const char* playback_dir; + FILE* playback_file; + int lineno; + + uint64_t next_time_us; +}; +typedef struct SurvivePlaybackData SurvivePlaybackData; + + +uint64_t timestamp_in_us() { + static uint64_t start_time_us = 0; + struct timeval tv; + gettimeofday(&tv,NULL); + uint64_t now = (uint64_t)tv.tv_sec * 1000000L + tv.tv_usec; + if(start_time_us == 0) + start_time_us = now; + return now - start_time_us; +} + +static int parse_and_run_imu(const char* line, SurvivePlaybackData* driver) { + char dev[10]; + int timecode = 0; + FLT accelgyro[6]; + int mask; + int id; + + int rr = sscanf(line,"I %s %d %d " FLT_format " " FLT_format " " FLT_format " " FLT_format " " FLT_format " " FLT_format "%d", dev, + &mask, + &timecode, + &accelgyro[0], &accelgyro[1], &accelgyro[2], + &accelgyro[3], &accelgyro[4], &accelgyro[5], &id ); + + if( rr != 10 ) + { + fprintf( stderr, "Warning: On line %d, only %d values read: '%s'\n", driver->lineno, rr, line ); + return -1; + } + + SurviveObject * so = survive_get_so_by_name( driver->ctx, dev); + if(!so) { + fprintf(stderr, "Could not find device named %s from lineno %d\n", dev, driver->lineno); + return -1; + } + + driver->ctx->imuproc( so, mask, accelgyro, timecode, id); + return 0; +} + + +static int parse_and_run_lightcode(const char* line, SurvivePlaybackData* driver) { + char lhn[10]; + char axn[10]; + char dev[10]; + uint32_t timecode = 0; + int sensor = 0; + int acode = 0; + int timeinsweep = 0; + uint32_t pulselength = 0; + uint32_t lh = 0; + + int rr = sscanf(line,"%8s %8s %8s %u %d %d %d %u %u\n", + lhn, axn, dev, + &timecode, &sensor, &acode, + &timeinsweep, &pulselength, &lh ); + + if( rr != 9 ) + { + fprintf( stderr, "Warning: On line %d, only %d values read: '%s'\n", driver->lineno, rr, line ); + return -1; + } + + SurviveObject * so = survive_get_so_by_name( driver->ctx, dev); + if(!so) { + fprintf(stderr, "Could not find device named %s from lineno %d\n", dev, driver->lineno); + return -1; + } + + driver->ctx->lightproc( so, sensor, acode, timeinsweep, timecode, pulselength, lh); + return 0; +} + +static int playback_poll( struct SurviveContext * ctx, void * _driver ) { + SurvivePlaybackData* driver = _driver; + FILE* f = driver->playback_file; + + if(f && !feof(f) && !ferror(f) ) + { + int i; + driver->lineno++; + char * line; + + if(driver->next_time_us == 0) { + char * buffer; + size_t n = 0; + ssize_t r = getdelim( &line, &n, ' ', f ); + if( r <= 0 ) return 0; + + uint64_t timestamp; + if(sscanf(line, "%lu", &driver->next_time_us) != 1) { + free(line); + return 0; + } + free(line); + line = 0; + } + + if(driver->next_time_us > timestamp_in_us()) + return 0; + driver->next_time_us = 0; + + char * buffer; + size_t n = 0; + ssize_t r = getline( &line, &n, f ); + if( r <= 0 ) return 0; + + if((line[0] != 'R' && line[0] != 'L' && line[0] != 'I') || line[1] != ' ' ) + return 0; + + switch(line[0]) { + case 'L': + case 'R': + parse_and_run_lightcode(line, driver); + break; + case 'I': + parse_and_run_imu(line, driver); + break; + } + + free( line ); + } else { + if(f) { + fclose(driver->playback_file); + } + driver->playback_file = 0; + return -1; + } + + return 0; +} + +int playback_close( struct SurviveContext * ctx, void * _driver ) { + SurvivePlaybackData* driver = _driver; + if(driver->playback_file) + fclose(driver->playback_file); + driver->playback_file = 0; + return 0; +} + + +static int LoadConfig( SurvivePlaybackData * sv, SurviveObject * so, int devno, int iface, int extra_magic ) +{ + SurviveContext * ctx = sv->ctx; + char * ct0conf = 0; + + char fname[100]; + sprintf( fname, "%s/%s_config.json", sv->playback_dir, so->codename ); + FILE * f = fopen( fname, "r" ); + + if(f == 0 || feof(f) || ferror(f) ) + return 1; + + fseek(f, 0, SEEK_END); + int len = ftell(f); + fseek(f, 0, SEEK_SET); //same as rewind(f); + + ct0conf = malloc(len+1); + fread( ct0conf, len, 1, f); + fclose( f ); + ct0conf[len] = 0; + + printf( "Loading config: %d\n", len ); + + if (len == 0) + return 1; + + //From JSMN example. + jsmn_parser p; + jsmntok_t t[4096]; + jsmn_init(&p); + int i; + int r = jsmn_parse(&p, ct0conf, len, t, sizeof(t)/sizeof(t[0])); + if (r < 0) { + SV_INFO("Failed to parse JSON in HMD configuration: %d\n", r); + return -1; + } + if (r < 1 || t[0].type != JSMN_OBJECT) { + SV_INFO("Object expected in HMD configuration\n"); + return -2; + } + + for (i = 1; i < r; i++) { + jsmntok_t * tk = &t[i]; + + char ctxo[100]; + int ilen = tk->end - tk->start; + if( ilen > 99 ) ilen = 99; + memcpy(ctxo, ct0conf + tk->start, ilen); + ctxo[ilen] = 0; + +// printf( "%d / %d / %d / %d %s %d\n", tk->type, tk->start, tk->end, tk->size, ctxo, jsoneq(ct0conf, &t[i], "modelPoints") ); +// printf( "%.*s\n", ilen, ct0conf + tk->start ); + + if (jsoneq(ct0conf, tk, "modelPoints") == 0) { + if( ParsePoints( ctx, so, ct0conf, &so->sensor_locations, t, i ) ) + { + break; + } + } + if (jsoneq(ct0conf, tk, "modelNormals") == 0) { + if( ParsePoints( ctx, so, ct0conf, &so->sensor_normals, t, i ) ) + { + break; + } + } + + + if (jsoneq(ct0conf, tk, "acc_bias") == 0) { + int32_t count = (tk+1)->size; + FLT* values = NULL; + if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { + so->acc_bias = values; + so->acc_bias[0] *= .125; //XXX Wat? Observed by CNL. Biasing by more than this seems to hose things. + so->acc_bias[1] *= .125; + so->acc_bias[2] *= .125; + } + } + if (jsoneq(ct0conf, tk, "acc_scale") == 0) { + int32_t count = (tk+1)->size; + FLT* values = NULL; + if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { + so->acc_scale = values; + } + } + + if (jsoneq(ct0conf, tk, "gyro_bias") == 0) { + int32_t count = (tk+1)->size; + FLT* values = NULL; + if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { + so->gyro_bias = values; + } + } + if (jsoneq(ct0conf, tk, "gyro_scale") == 0) { + int32_t count = (tk+1)->size; + FLT* values = NULL; + if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) { + so->gyro_scale = values; + } + } + } + + return 0; +} + + + +int DriverRegPlayback( SurviveContext * ctx ) +{ + const char* playback_dir = config_read_str(ctx->global_config_values, + "PlaybackDir", ""); + + if(strlen(playback_dir) == 0) { + return 0; +} + + SurvivePlaybackData * sp = calloc( 1, sizeof( SurvivePlaybackData ) ); + sp->ctx = ctx; + sp->playback_dir = playback_dir; + printf("%s\n", playback_dir); + + char playback_file[100]; + sprintf( playback_file, "%s/events", playback_dir ); + sp->playback_file = fopen( playback_file, "r"); + if(sp->playback_file == 0) { + fprintf(stderr, "Could not open playback events file %s", playback_file); + return -1; +} + SurviveObject * hmd = survive_create_hmd(ctx, "Playback", sp); + SurviveObject * wm0 = survive_create_wm0(ctx, "Playback", sp, 0); + SurviveObject * wm1 = survive_create_wm1(ctx, "Playback", sp, 0); + SurviveObject * tr0 = survive_create_tr0(ctx, "Playback", sp); + SurviveObject * ww0 = survive_create_ww0(ctx, "Playback", sp); + + if( !LoadConfig( sp, hmd, 1, 0, 0 )) { survive_add_object( ctx, hmd ); } + if( !LoadConfig( sp, wm0, 2, 0, 1 )) { survive_add_object( ctx, wm0 ); } + if( !LoadConfig( sp, wm1, 3, 0, 1 )) { survive_add_object( ctx, wm1 ); } + if( !LoadConfig( sp, tr0, 4, 0, 0 )) { survive_add_object( ctx, tr0 ); } + if( !LoadConfig( sp, ww0, 5, 0, 0 )) { survive_add_object( ctx, ww0 ); } + + + survive_add_driver(ctx, sp, playback_poll, playback_close, 0); + return 0; + fail_gracefully: + return -1; +} + +REGISTER_LINKTIME( DriverRegPlayback ); + diff --git a/src/survive_vive.c b/src/survive_vive.c index cb05efc..e21d419 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -23,6 +23,7 @@ #endif #include "json_helpers.h" +#include "survive_default_devices.h" #ifdef HIDAPI #if defined(WINDOWS) || defined(WIN32) || defined (_WIN32) @@ -1699,15 +1700,16 @@ static int LoadConfig( SurviveViveData * sv, SurviveObject * so, int devno, int SurviveContext * ctx = sv->ctx; char * ct0conf = 0; int len = survive_get_config( &ct0conf, sv, devno, iface, extra_magic ); -printf( "Loading config: %d\n", len ); -#if 0 - char fname[100]; - sprintf( fname, "%s_config.json", so->codename ); - FILE * f = fopen( fname, "w" ); - fwrite( ct0conf, strlen(ct0conf), 1, f ); - fclose( f ); -#endif + printf( "Loading config: %d\n", len ); + { + char raw_fname[100]; + sprintf( raw_fname, "%s_config.json", so->codename ); + FILE * f = fopen( raw_fname, "w" ); + fwrite( ct0conf, strlen(ct0conf), 1, f ); + fclose( f ); + } + if( len > 0 ) { @@ -1836,18 +1838,13 @@ void init_SurviveObject(SurviveObject* so) { int DriverRegHTCVive( SurviveContext * ctx ) { int r; - SurviveObject * hmd = calloc( 1, sizeof( SurviveObject ) ); - SurviveObject * wm0 = calloc( 1, sizeof( SurviveObject ) ); - SurviveObject * wm1 = calloc( 1, sizeof( SurviveObject ) ); - SurviveObject * tr0 = calloc( 1, sizeof( SurviveObject ) ); - SurviveObject * ww0 = calloc( 1, sizeof( SurviveObject ) ); - SurviveViveData * sv = calloc( 1, sizeof( SurviveViveData ) ); - - init_SurviveObject(hmd); - init_SurviveObject(wm0); - init_SurviveObject(wm1); - init_SurviveObject(tr0); - init_SurviveObject(ww0); + + SurviveViveData * sv = calloc(1, sizeof(SurviveViveData) ); + SurviveObject * hmd = survive_create_hmd(ctx, "HTC", sv); + SurviveObject * wm0 = survive_create_wm0(ctx, "HTC", sv, 0); + SurviveObject * wm1 = survive_create_wm1(ctx, "HTC", sv, 0); + SurviveObject * tr0 = survive_create_tr0(ctx, "HTC", sv); + SurviveObject * ww0 = survive_create_ww0(ctx, "HTC", sv); sv->ctx = ctx; @@ -1859,28 +1856,6 @@ int DriverRegHTCVive( SurviveContext * ctx ) mkdir( "calinfo", 0755 ); #endif - - hmd->ctx = ctx; - hmd->driver = sv; - memcpy( hmd->codename, "HMD", 4 ); - memcpy( hmd->drivername, "HTC", 4 ); - wm0->ctx = ctx; - wm0->driver = sv; - memcpy( wm0->codename, "WM0", 4 ); - memcpy( wm0->drivername, "HTC", 4 ); - wm1->ctx = ctx; - wm1->driver = sv; - memcpy( wm1->codename, "WM1", 4 ); - memcpy( wm1->drivername, "HTC", 4 ); - tr0->ctx = ctx; - tr0->driver = sv; - memcpy( tr0->codename, "TR0", 4 ); - memcpy( tr0->drivername, "HTC", 4 ); - ww0->ctx = ctx; - ww0->driver = sv; - memcpy( ww0->codename, "WW0", 4 ); - memcpy( ww0->drivername, "HTC", 4 ); - //USB must happen last. if( r = survive_usb_init( sv, hmd, wm0, wm1, tr0, ww0) ) { @@ -1894,36 +1869,6 @@ int DriverRegHTCVive( SurviveContext * ctx ) if( sv->udev[USB_DEV_WATCHMAN2] && LoadConfig( sv, wm1, 3, 0, 1 )) { SV_INFO( "Watchman 1 config issue." ); } if( sv->udev[USB_DEV_TRACKER0] && LoadConfig( sv, tr0, 4, 0, 0 )) { SV_INFO( "Tracker 0 config issue." ); } if( sv->udev[USB_DEV_W_WATCHMAN1] && LoadConfig( sv, ww0, 5, 0, 0 )) { SV_INFO( "Wired Watchman 0 config issue." ); } - - hmd->timebase_hz = wm0->timebase_hz = wm1->timebase_hz = 48000000; - tr0->timebase_hz = ww0->timebase_hz = hmd->timebase_hz; - - hmd->pulsedist_max_ticks = wm0->pulsedist_max_ticks = wm1->pulsedist_max_ticks = 500000; - tr0->pulsedist_max_ticks = ww0->pulsedist_max_ticks = hmd->pulsedist_max_ticks; - - hmd->pulselength_min_sync = wm0->pulselength_min_sync = wm1->pulselength_min_sync = 2200; - tr0->pulselength_min_sync = ww0->pulselength_min_sync = hmd->pulselength_min_sync; - - hmd->pulse_in_clear_time = wm0->pulse_in_clear_time = wm1->pulse_in_clear_time = 35000; - tr0->pulse_in_clear_time = ww0->pulse_in_clear_time = hmd->pulse_in_clear_time; - - hmd->pulse_max_for_sweep = wm0->pulse_max_for_sweep = wm1->pulse_max_for_sweep = 1800; - tr0->pulse_max_for_sweep = ww0->pulse_max_for_sweep = hmd->pulse_max_for_sweep; - - hmd->pulse_synctime_offset = wm0->pulse_synctime_offset = wm1->pulse_synctime_offset = 20000; - tr0->pulse_synctime_offset = ww0->pulse_synctime_offset = hmd->pulse_synctime_offset; - - hmd->pulse_synctime_slack = wm0->pulse_synctime_slack = wm1->pulse_synctime_slack = 5000; - tr0->pulse_synctime_slack = ww0->pulse_synctime_slack = hmd->pulse_synctime_slack; - - hmd->timecenter_ticks = hmd->timebase_hz / 240; - wm0->timecenter_ticks = wm0->timebase_hz / 240; - wm1->timecenter_ticks = wm1->timebase_hz / 240; - tr0->timecenter_ticks = tr0->timebase_hz / 240; - ww0->timecenter_ticks = ww0->timebase_hz / 240; - - wm0->haptic = survive_vive_send_haptic; - wm1->haptic = survive_vive_send_haptic; /* int i; int locs = hmd->nr_locations; @@ -1945,7 +1890,7 @@ int DriverRegHTCVive( SurviveContext * ctx ) } } */ - + //Add the drivers. if( sv->udev[USB_DEV_HMD_IMU_LH] ) { survive_add_object( ctx, hmd ); } if( sv->udev[USB_DEV_WATCHMAN1] ) { survive_add_object( ctx, wm0 ); } @@ -1953,7 +1898,15 @@ int DriverRegHTCVive( SurviveContext * ctx ) if( sv->udev[USB_DEV_TRACKER0] ) { survive_add_object( ctx, tr0 ); } if( sv->udev[USB_DEV_W_WATCHMAN1] ) { survive_add_object( ctx, ww0 ); } - survive_add_driver( ctx, sv, survive_vive_usb_poll, survive_vive_close, survive_vive_send_magic ); + if( sv->udev[USB_DEV_HMD_IMU_LH] || + sv->udev[USB_DEV_WATCHMAN1] || + sv->udev[USB_DEV_WATCHMAN2] || + sv->udev[USB_DEV_TRACKER0] || + sv->udev[USB_DEV_W_WATCHMAN1] ) { + survive_add_driver( ctx, sv, survive_vive_usb_poll, survive_vive_close, survive_vive_send_magic ); + } else { + fprintf(stderr, "No USB devices detected\n"); + } return 0; fail_gracefully: -- cgit v1.3.1