diff options
-rw-r--r-- | Makefile | 2 | ||||
-rw-r--r-- | README.md | 2 | ||||
-rw-r--r-- | data_recorder.c | 259 | ||||
-rw-r--r-- | include/libsurvive/survive_types.h | 6 | ||||
-rw-r--r-- | redist/hid-linux.c | 798 | ||||
-rw-r--r-- | src/survive_default_devices.c | 199 | ||||
-rw-r--r-- | src/survive_default_devices.h | 19 | ||||
-rw-r--r-- | src/survive_internal.h | 1 | ||||
-rw-r--r-- | src/survive_playback.c | 242 | ||||
-rwxr-xr-x | src/survive_vive.c | 277 | ||||
-rw-r--r-- | useful_files/81-vive.rules | 10 |
11 files changed, 1454 insertions, 361 deletions
@@ -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. @@ -79,6 +79,8 @@ Will ~~I~~ we succeed? Probably not. ~~Definitely going to try!~~ Though it's * pthread * libX11 (Linux) or Native (win32) or OpenGL (OSX) * zlib (Linux) or puff.c (win32, included in redist) +* Optionally OpenGL. +On Debian, ```sudo apt-get install build-essential zlib1g-dev libx11-dev libusb-1.0-0-dev freeglut3-dev``` should be sufficient. ## Architecture diff --git a/data_recorder.c b/data_recorder.c index 002357e..6123d57 100644 --- a/data_recorder.c +++ b/data_recorder.c @@ -1,177 +1,204 @@ -//Data recorder mod with GUI showing light positions. +// Data recorder mod with GUI showing light positions. #ifdef __linux__ #include <unistd.h> #endif + +#include <CNFGFunctions.h> +#include <os_generic.h> +#include <stdarg.h> +#include <stdint.h> #include <stdio.h> #include <stdlib.h> -#include <stdint.h> -#include <survive.h> #include <string.h> -#include <os_generic.h> -#include <CNFGFunctions.h> +#include <survive.h> +#include <sys/time.h> +#include <time.h> -struct SurviveContext * ctx; +#include "redist/os_generic.h" -void HandleKey( int keycode, int bDown ) -{ - if( !bDown ) return; +struct SurviveContext *ctx; - if( keycode == 'O' || keycode == 'o' ) - { - survive_send_magic(ctx,1,0,0); +FILE *output_file = 0; + +void HandleKey(int keycode, int bDown) { + if (!bDown) + return; + + if (keycode == 'O' || keycode == 'o') { + survive_send_magic(ctx, 1, 0, 0); } - if( keycode == 'F' || keycode == 'f' ) - { - survive_send_magic(ctx,0,0,0); + if (keycode == 'F' || keycode == 'f') { + survive_send_magic(ctx, 0, 0, 0); } } -void HandleButton( int x, int y, int button, int bDown ) -{ -} - -void HandleMotion( int x, int y, int mask ) -{ -} +void HandleButton(int x, int y, int button, int bDown) {} -void HandleDestroy() -{ -} +void HandleMotion(int x, int y, int mask) {} -int bufferpts[32*2*3]; -char buffermts[32*128*3]; -int buffertimeto[32*3]; +void HandleDestroy() {} -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); +int bufferpts[32 * 2 * 3]; +char buffermts[32 * 128 * 3]; +int buffertimeto[32 * 3]; - if( acode == -1 ) return; -//return; - int jumpoffset = sensor_id; - if( strcmp( so->codename, "WM0" ) == 0 ) jumpoffset += 32; - else if( strcmp( so->codename, "WM1" ) == 0 ) jumpoffset += 64; +double timestamp_in_us() { + static double start_time_us = 0; + if (start_time_us == 0) + start_time_us = OGGetAbsoluteTime(); + return OGGetAbsoluteTime() - start_time_us; +} +int write_to_output(const char *format, ...) { + va_list args; + va_start(args, format); + fprintf(output_file, "%.17g ", timestamp_in_us()); + vfprintf(output_file, format, args); - 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; - } + 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 == 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 == -1) { + write_to_output("A %s %d %d %d %u %u %u\n", so->codename, sensor_id, + acode, timeinsweep, timecode, length, lh); + return; } - 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; + + int jumpoffset = sensor_id; + if (strcmp(so->codename, "WM0") == 0) + jumpoffset += 32; + else if (strcmp(so->codename, "WM1") == 0) + jumpoffset += 64; + + 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 ); - } +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); + 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); } - - - -void * GuiThread( void * v ) -{ +void *GuiThread(void *v) { CNFGBGColor = 0x000000; CNFGDialogColor = 0x444444; - CNFGSetup( "Survive GUI Debug", 640, 480 ); + CNFGSetup("Survive GUI Debug", 640, 480); short screenx, screeny; - while(1) - { + while (1) { CNFGHandleInput(); CNFGClearFrame(); - CNFGColor( 0xFFFFFF ); - CNFGGetDimensions( &screenx, &screeny ); + CNFGColor(0xFFFFFF); + CNFGGetDimensions(&screenx, &screeny); int i; - for( i = 0; i < 32*3; i++ ) - { - if( buffertimeto[i] < 50 ) - { + for (i = 0; i < 32 * 3; i++) { + if (buffertimeto[i] < 50) { uint32_t color = i * 3231349; uint8_t r = color & 0xff; - uint8_t g = (color>>8) & 0xff; - uint8_t b = (color>>16) & 0xff; - r = (r * (5-buffertimeto[i])) / 5 ; - g = (g * (5-buffertimeto[i])) / 5 ; - b = (b * (5-buffertimeto[i])) / 5 ; - CNFGColor( (b<<16) | (g<<8) | r ); - CNFGTackRectangle( bufferpts[i*2+0], bufferpts[i*2+1], bufferpts[i*2+0] + 5, bufferpts[i*2+1] + 5 ); - CNFGPenX = bufferpts[i*2+0]; CNFGPenY = bufferpts[i*2+1]; - CNFGDrawText( buffermts, 2 ); + uint8_t g = (color >> 8) & 0xff; + uint8_t b = (color >> 16) & 0xff; + r = (r * (5 - buffertimeto[i])) / 5; + g = (g * (5 - buffertimeto[i])) / 5; + b = (b * (5 - buffertimeto[i])) / 5; + CNFGColor((b << 16) | (g << 8) | r); + CNFGTackRectangle(bufferpts[i * 2 + 0], bufferpts[i * 2 + 1], + bufferpts[i * 2 + 0] + 5, + bufferpts[i * 2 + 1] + 5); + CNFGPenX = bufferpts[i * 2 + 0]; + CNFGPenY = bufferpts[i * 2 + 1]; + CNFGDrawText(buffermts, 2); buffertimeto[i]++; } } CNFGSwapBuffers(); - OGUSleep( 10000 ); + OGUSleep(10000); } } -int SurviveThreadLoaded=0; +int SurviveThreadLoaded = 0; -void *SurviveThread(void *junk) -{ - ctx = survive_init( 0 ); +void *SurviveThread(void *junk) { + ctx = survive_init(0); - survive_install_light_fn( ctx, my_light_process ); - survive_install_imu_fn( ctx, my_imu_process ); + survive_install_light_fn(ctx, my_light_process); + survive_install_imu_fn(ctx, my_imu_process); - if( !ctx ) - { - fprintf( stderr, "Fatal. Could not start\n" ); + if (!ctx) { + fprintf(stderr, "Fatal. Could not start\n"); exit(1); } SurviveThreadLoaded = 1; - while(survive_poll(ctx) == 0) - { - printf("Do stuff.\n"); - //Do stuff. + while (survive_poll(ctx) == 0) { } - return 0; + return 0; } -int main() -{ - // Create the libsurvive thread - OGCreateThread(SurviveThread, 0); - +int main(int argc, char **argv) { + 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); } - - // Run the Gui in the main thread - GuiThread(0); -} + while (!SurviveThreadLoaded) { + OGUSleep(100); + } + // 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/redist/hid-linux.c b/redist/hid-linux.c new file mode 100644 index 0000000..5cc3bf2 --- /dev/null +++ b/redist/hid-linux.c @@ -0,0 +1,798 @@ +/******************************************************* + HIDAPI - Multi-Platform library for + communication with HID devices. + + Alan Ott + Signal 11 Software + + 8/22/2009 + Linux Version - 6/2/2009 + + Copyright 2009, All Rights Reserved. + + At the discretion of the user of this library, + this software may be licensed under the terms of the + GNU General Public License v3, a BSD-Style license, or the + original HIDAPI license as outlined in the LICENSE.txt, + LICENSE-gpl3.txt, LICENSE-bsd.txt, and LICENSE-orig.txt + files located at the root of the source distribution. + These files may also be found in the public source + code repository located at: + http://github.com/signal11/hidapi . +********************************************************/ + +/* C */ +#include <stdio.h> +#include <string.h> +#include <stdlib.h> +#include <locale.h> +#include <errno.h> + +/* Unix */ +#include <unistd.h> +#include <sys/types.h> +#include <sys/stat.h> +#include <sys/ioctl.h> +#include <sys/utsname.h> +#include <fcntl.h> +#include <poll.h> + +/* Linux */ +#include <linux/hidraw.h> +#include <linux/version.h> +#include <linux/input.h> +#include <libudev.h> + +#include "hidapi.h" + +/* Definitions from linux/hidraw.h. Since these are new, some distros + may not have header files which contain them. */ +#ifndef HIDIOCSFEATURE +#define HIDIOCSFEATURE(len) _IOC(_IOC_WRITE|_IOC_READ, 'H', 0x06, len) +#endif +#ifndef HIDIOCGFEATURE +#define HIDIOCGFEATURE(len) _IOC(_IOC_WRITE|_IOC_READ, 'H', 0x07, len) +#endif + + +/* USB HID device property names */ +const char *device_string_names[] = { + "manufacturer", + "product", + "serial", +}; + +/* Symbolic names for the properties above */ +enum device_string_id { + DEVICE_STRING_MANUFACTURER, + DEVICE_STRING_PRODUCT, + DEVICE_STRING_SERIAL, + + DEVICE_STRING_COUNT, +}; + +struct hid_device_ { + int device_handle; + int blocking; + int uses_numbered_reports; +}; + + +static __u32 kernel_version = 0; + +static __u32 detect_kernel_version(void) +{ + struct utsname name; + int major, minor, release; + int ret; + + uname(&name); + ret = sscanf(name.release, "%d.%d.%d", &major, &minor, &release); + if (ret == 3) { + return KERNEL_VERSION(major, minor, release); + } + + ret = sscanf(name.release, "%d.%d", &major, &minor); + if (ret == 2) { + return KERNEL_VERSION(major, minor, 0); + } + + printf("Couldn't determine kernel version from version string \"%s\"\n", name.release); + return 0; +} + +static hid_device *new_hid_device(void) +{ + hid_device *dev = calloc(1, sizeof(hid_device)); + dev->device_handle = -1; + dev->blocking = 1; + dev->uses_numbered_reports = 0; + + return dev; +} + + +/* The caller must free the returned string with free(). */ +static wchar_t *utf8_to_wchar_t(const char *utf8) +{ + wchar_t *ret = NULL; + + if (utf8) { + size_t wlen = mbstowcs(NULL, utf8, 0); + if ((size_t) -1 == wlen) { + return wcsdup(L""); + } + ret = calloc(wlen+1, sizeof(wchar_t)); + mbstowcs(ret, utf8, wlen+1); + ret[wlen] = 0x0000; + } + + return ret; +} + +/* Get an attribute value from a udev_device and return it as a whar_t + string. The returned string must be freed with free() when done.*/ +static wchar_t *copy_udev_string(struct udev_device *dev, const char *udev_name) +{ + return utf8_to_wchar_t(udev_device_get_sysattr_value(dev, udev_name)); +} + +/* uses_numbered_reports() returns 1 if report_descriptor describes a device + which contains numbered reports. */ +static int uses_numbered_reports(__u8 *report_descriptor, __u32 size) { + unsigned int i = 0; + int size_code; + int data_len, key_size; + + while (i < size) { + int key = report_descriptor[i]; + + /* Check for the Report ID key */ + if (key == 0x85/*Report ID*/) { + /* This device has a Report ID, which means it uses + numbered reports. */ + return 1; + } + + //printf("key: %02hhx\n", key); + + if ((key & 0xf0) == 0xf0) { + /* This is a Long Item. The next byte contains the + length of the data section (value) for this key. + See the HID specification, version 1.11, section + 6.2.2.3, titled "Long Items." */ + if (i+1 < size) + data_len = report_descriptor[i+1]; + else + data_len = 0; /* malformed report */ + key_size = 3; + } + else { + /* This is a Short Item. The bottom two bits of the + key contain the size code for the data section + (value) for this key. Refer to the HID + specification, version 1.11, section 6.2.2.2, + titled "Short Items." */ + size_code = key & 0x3; + switch (size_code) { + case 0: + case 1: + case 2: + data_len = size_code; + break; + case 3: + data_len = 4; + break; + default: + /* Can't ever happen since size_code is & 0x3 */ + data_len = 0; + break; + }; + key_size = 1; + } + + /* Skip over this key and it's associated data */ + i += data_len + key_size; + } + + /* Didn't find a Report ID key. Device doesn't use numbered reports. */ + return 0; +} + +/* + * The caller is responsible for free()ing the (newly-allocated) character + * strings pointed to by serial_number_utf8 and product_name_utf8 after use. + */ +static int +parse_uevent_info(const char *uevent, int *bus_type, + unsigned short *vendor_id, unsigned short *product_id, + char **serial_number_utf8, char **product_name_utf8) +{ + char *tmp = strdup(uevent); + char *saveptr = NULL; + char *line; + char *key; + char *value; + + int found_id = 0; + int found_serial = 0; + int found_name = 0; + + line = strtok_r(tmp, "\n", &saveptr); + while (line != NULL) { + /* line: "KEY=value" */ + key = line; + value = strchr(line, '='); + if (!value) { + goto next_line; + } + *value = '\0'; + value++; + + if (strcmp(key, "HID_ID") == 0) { + /** + * type vendor product + * HID_ID=0003:000005AC:00008242 + **/ + int ret = sscanf(value, "%x:%hx:%hx", bus_type, vendor_id, product_id); + if (ret == 3) { + found_id = 1; + } + } else if (strcmp(key, "HID_NAME") == 0) { + /* The caller has to free the product name */ + *product_name_utf8 = strdup(value); + found_name = 1; + } else if (strcmp(key, "HID_UNIQ") == 0) { + /* The caller has to free the serial number */ + *serial_number_utf8 = strdup(value); + found_serial = 1; + } + +next_line: + line = strtok_r(NULL, "\n", &saveptr); + } + + free(tmp); + return (found_id && found_name && found_serial); +} + + +static int get_device_string(hid_device *dev, enum device_string_id key, wchar_t *string, size_t maxlen) +{ + struct udev *udev; + struct udev_device *udev_dev, *parent, *hid_dev; + struct stat s; + int ret = -1; + char *serial_number_utf8 = NULL; + char *product_name_utf8 = NULL; + + /* Create the udev object */ + udev = udev_new(); + if (!udev) { + printf("Can't create udev\n"); + return -1; + } + + /* Get the dev_t (major/minor numbers) from the file handle. */ + ret = fstat(dev->device_handle, &s); + if (-1 == ret) + return ret; + /* Open a udev device from the dev_t. 'c' means character device. */ + udev_dev = udev_device_new_from_devnum(udev, 'c', s.st_rdev); + if (udev_dev) { + hid_dev = udev_device_get_parent_with_subsystem_devtype( + udev_dev, + "hid", + NULL); + if (hid_dev) { + unsigned short dev_vid; + unsigned short dev_pid; + int bus_type; + size_t retm; + + ret = parse_uevent_info( + udev_device_get_sysattr_value(hid_dev, "uevent"), + &bus_type, + &dev_vid, + &dev_pid, + &serial_number_utf8, + &product_name_utf8); + + if (bus_type == BUS_BLUETOOTH) { + switch (key) { + case DEVICE_STRING_MANUFACTURER: + wcsncpy(string, L"", maxlen); + ret = 0; + break; + case DEVICE_STRING_PRODUCT: + retm = mbstowcs(string, product_name_utf8, maxlen); + ret = (retm == (size_t)-1)? -1: 0; + break; + case DEVICE_STRING_SERIAL: + retm = mbstowcs(string, serial_number_utf8, maxlen); + ret = (retm == (size_t)-1)? -1: 0; + break; + case DEVICE_STRING_COUNT: + default: + ret = -1; + break; + } + } + else { + /* This is a USB device. Find its parent USB Device node. */ + parent = udev_device_get_parent_with_subsystem_devtype( + udev_dev, + "usb", + "usb_device"); + if (parent) { + const char *str; + const char *key_str = NULL; + + if (key >= 0 && key < DEVICE_STRING_COUNT) { + key_str = device_string_names[key]; + } else { + ret = -1; + goto end; + } + + str = udev_device_get_sysattr_value(parent, key_str); + if (str) { + /* Convert the string from UTF-8 to wchar_t */ + retm = mbstowcs(string, str, maxlen); + ret = (retm == (size_t)-1)? -1: 0; + goto end; + } + } + } + } + } + +end: + free(serial_number_utf8); + free(product_name_utf8); + + udev_device_unref(udev_dev); + /* parent and hid_dev don't need to be (and can't be) unref'd. + I'm not sure why, but they'll throw double-free() errors. */ + udev_unref(udev); + + return ret; +} + +int HID_API_EXPORT hid_init(void) +{ + const char *locale; + + /* Set the locale if it's not set. */ + locale = setlocale(LC_CTYPE, NULL); + if (!locale) + setlocale(LC_CTYPE, ""); + + kernel_version = detect_kernel_version(); + + return 0; +} + +int HID_API_EXPORT hid_exit(void) +{ + /* Nothing to do for this in the Linux/hidraw implementation. */ + return 0; +} + + +struct hid_device_info HID_API_EXPORT *hid_enumerate(unsigned short vendor_id, unsigned short product_id) +{ + struct udev *udev; + struct udev_enumerate *enumerate; + struct udev_list_entry *devices, *dev_list_entry; + + struct hid_device_info *root = NULL; /* return object */ + struct hid_device_info *cur_dev = NULL; + struct hid_device_info *prev_dev = NULL; /* previous device */ + + hid_init(); + + /* Create the udev object */ + udev = udev_new(); + if (!udev) { + printf("Can't create udev\n"); + return NULL; + } + + /* Create a list of the devices in the 'hidraw' subsystem. */ + enumerate = udev_enumerate_new(udev); + udev_enumerate_add_match_subsystem(enumerate, "hidraw"); + udev_enumerate_scan_devices(enumerate); + devices = udev_enumerate_get_list_entry(enumerate); + /* For each item, see if it matches the vid/pid, and if so + create a udev_device record for it */ + udev_list_entry_foreach(dev_list_entry, devices) { + const char *sysfs_path; + const char *dev_path; + const char *str; + struct udev_device *raw_dev; /* The device's hidraw udev node. */ + struct udev_device *hid_dev; /* The device's HID udev node. */ + struct udev_device *usb_dev; /* The device's USB udev node. */ + struct udev_device *intf_dev; /* The device's interface (in the USB sense). */ + unsigned short dev_vid; + unsigned short dev_pid; + char *serial_number_utf8 = NULL; + char *product_name_utf8 = NULL; + int bus_type; + int result; + + /* Get the filename of the /sys entry for the device + and create a udev_device object (dev) representing it */ + sysfs_path = udev_list_entry_get_name(dev_list_entry); + raw_dev = udev_device_new_from_syspath(udev, sysfs_path); + dev_path = udev_device_get_devnode(raw_dev); + + hid_dev = udev_device_get_parent_with_subsystem_devtype( + raw_dev, + "hid", + NULL); + + if (!hid_dev) { + /* Unable to find parent hid device. */ + goto next; + } + + result = parse_uevent_info( + udev_device_get_sysattr_value(hid_dev, "uevent"), + &bus_type, + &dev_vid, + &dev_pid, + &serial_number_utf8, + &product_name_utf8); + + if (!result) { + /* parse_uevent_info() failed for at least one field. */ + goto next; + } + + if (bus_type != BUS_USB && bus_type != BUS_BLUETOOTH) { + /* We only know how to handle USB and BT devices. */ + goto next; + } + + /* Check the VID/PID against the arguments */ + if ((vendor_id == 0x0 || vendor_id == dev_vid) && + (product_id == 0x0 || product_id == dev_pid)) { + struct hid_device_info *tmp; + + /* VID/PID match. Create the record. */ + tmp = malloc(sizeof(struct hid_device_info)); + if (cur_dev) { + cur_dev->next = tmp; + } + else { + root = tmp; + } + prev_dev = cur_dev; + cur_dev = tmp; + + /* Fill out the record */ + cur_dev->next = NULL; + cur_dev->path = dev_path? strdup(dev_path): NULL; + + /* VID/PID */ + cur_dev->vendor_id = dev_vid; + cur_dev->product_id = dev_pid; + + /* Serial Number */ + cur_dev->serial_number = utf8_to_wchar_t(serial_number_utf8); + + /* Release Number */ + cur_dev->release_number = 0x0; + + /* Interface Number */ + cur_dev->interface_number = -1; + + switch (bus_type) { + case BUS_USB: + /* The device pointed to by raw_dev contains information about + the hidraw device. In order to get information about the + USB device, get the parent device with the + subsystem/devtype pair of "usb"/"usb_device". This will + be several levels up the tree, but the function will find + it. */ + usb_dev = udev_device_get_parent_with_subsystem_devtype( + raw_dev, + "usb", + "usb_device"); + + if (!usb_dev) { + /* Free this device */ + free(cur_dev->serial_number); + free(cur_dev->path); + free(cur_dev); + + /* Take it off the device list. */ + if (prev_dev) { + prev_dev->next = NULL; + cur_dev = prev_dev; + } + else { + cur_dev = root = NULL; + } + + goto next; + } + + /* Manufacturer and Product strings */ + cur_dev->manufacturer_string = copy_udev_string(usb_dev, device_string_names[DEVICE_STRING_MANUFACTURER]); + cur_dev->product_string = copy_udev_string(usb_dev, device_string_names[DEVICE_STRING_PRODUCT]); + + /* Release Number */ + str = udev_device_get_sysattr_value(usb_dev, "bcdDevice"); + cur_dev->release_number = (str)? strtol(str, NULL, 16): 0x0; + + /* Get a handle to the interface's udev node. */ + intf_dev = udev_device_get_parent_with_subsystem_devtype( + raw_dev, + "usb", + "usb_interface"); + if (intf_dev) { + str = udev_device_get_sysattr_value(intf_dev, "bInterfaceNumber"); + cur_dev->interface_number = (str)? strtol(str, NULL, 16): -1; + } + + break; + + case BUS_BLUETOOTH: + /* Manufacturer and Product strings */ + cur_dev->manufacturer_string = wcsdup(L""); + cur_dev->product_string = utf8_to_wchar_t(product_name_utf8); + + break; + + default: + /* Unknown device type - this should never happen, as we + * check for USB and Bluetooth devices above */ + break; + } + } + + next: + free(serial_number_utf8); + free(product_name_utf8); + udev_device_unref(raw_dev); + /* hid_dev, usb_dev and intf_dev don't need to be (and can't be) + unref()d. It will cause a double-free() error. I'm not + sure why. */ + } + /* Free the enumerator and udev objects. */ + udev_enumerate_unref(enumerate); + udev_unref(udev); + + return root; +} + +void HID_API_EXPORT hid_free_enumeration(struct hid_device_info *devs) +{ + struct hid_device_info *d = devs; + while (d) { + struct hid_device_info *next = d->next; + free(d->path); + free(d->serial_number); + free(d->manufacturer_string); + free(d->product_string); + free(d); + d = next; + } +} + +hid_device * hid_open(unsigned short vendor_id, unsigned short product_id, const wchar_t *serial_number) +{ + struct hid_device_info *devs, *cur_dev; + const char *path_to_open = NULL; + hid_device *handle = NULL; + + devs = hid_enumerate(vendor_id, product_id); + cur_dev = devs; + while (cur_dev) { + if (cur_dev->vendor_id == vendor_id && + cur_dev->product_id == product_id) { + if (serial_number) { + if (wcscmp(serial_number, cur_dev->serial_number) == 0) { + path_to_open = cur_dev->path; + break; + } + } + else { + path_to_open = cur_dev->path; + break; + } + } + cur_dev = cur_dev->next; + } + + if (path_to_open) { + /* Open the device */ + handle = hid_open_path(path_to_open); + } + + hid_free_enumeration(devs); + + return handle; +} + +hid_device * HID_API_EXPORT hid_open_path(const char *path) +{ + hid_device *dev = NULL; + + hid_init(); + + dev = new_hid_device(); + + /* OPEN HERE */ + dev->device_handle = open(path, O_RDWR); + + /* If we have a good handle, return it. */ + if (dev->device_handle > 0) { + + /* Get the report descriptor */ + int res, desc_size = 0; + struct hidraw_report_descriptor rpt_desc; + + memset(&rpt_desc, 0x0, sizeof(rpt_desc)); + + /* Get Report Descriptor Size */ + res = ioctl(dev->device_handle, HIDIOCGRDESCSIZE, &desc_size); + if (res < 0) + perror("HIDIOCGRDESCSIZE"); + + + /* Get Report Descriptor */ + rpt_desc.size = desc_size; + res = ioctl(dev->device_handle, HIDIOCGRDESC, &rpt_desc); + if (res < 0) { + perror("HIDIOCGRDESC"); + } else { + /* Determine if this device uses numbered reports. */ + dev->uses_numbered_reports = + uses_numbered_reports(rpt_desc.value, + rpt_desc.size); + } + + return dev; + } + else { + /* Unable to open any devices. */ + free(dev); + return NULL; + } +} + + +int HID_API_EXPORT hid_write(hid_device *dev, const unsigned char *data, size_t length) +{ + int bytes_written; + + bytes_written = write(dev->device_handle, data, length); + + return bytes_written; +} + + +int HID_API_EXPORT hid_read_timeout(hid_device *dev, unsigned char *data, size_t length, int milliseconds) +{ + int bytes_read; + + if (milliseconds >= 0) { + /* Milliseconds is either 0 (non-blocking) or > 0 (contains + a valid timeout). In both cases we want to call poll() + and wait for data to arrive. Don't rely on non-blocking + operation (O_NONBLOCK) since some kernels don't seem to + properly report device disconnection through read() when + in non-blocking mode. */ + int ret; + struct pollfd fds; + + fds.fd = dev->device_handle; + fds.events = POLLIN; + fds.revents = 0; + ret = poll(&fds, 1, milliseconds); + if (ret == -1 || ret == 0) { + /* Error or timeout */ + return ret; + } + else { + /* Check for errors on the file descriptor. This will + indicate a device disconnection. */ + if (fds.revents & (POLLERR | POLLHUP | POLLNVAL)) + return -1; + } + } + + bytes_read = read(dev->device_handle, data, length); + if (bytes_read < 0 && (errno == EAGAIN || errno == EINPROGRESS)) + bytes_read = 0; + + if (bytes_read >= 0 && + kernel_version != 0 && + kernel_version < KERNEL_VERSION(2,6,34) && + dev->uses_numbered_reports) { + /* Work around a kernel bug. Chop off the first byte. */ + memmove(data, data+1, bytes_read); + bytes_read--; + } + + return bytes_read; +} + +int HID_API_EXPORT hid_read(hid_device *dev, unsigned char *data, size_t length) +{ + return hid_read_timeout(dev, data, length, (dev->blocking)? -1: 0); +} + +int HID_API_EXPORT hid_set_nonblocking(hid_device *dev, int nonblock) +{ + /* Do all non-blocking in userspace using poll(), since it looks + like there's a bug in the kernel in some versions where + read() will not return -1 on disconnection of the USB device */ + + dev->blocking = !nonblock; + return 0; /* Success */ +} + + +int HID_API_EXPORT hid_send_feature_report(hid_device *dev, const unsigned char *data, size_t length) +{ + int res; + + res = ioctl(dev->device_handle, HIDIOCSFEATURE(length), data); + if (res < 0) + perror("ioctl (SFEATURE)"); + + return res; +} + +int HID_API_EXPORT hid_get_feature_report(hid_device *dev, unsigned char *data, size_t length) +{ + int res; + + res = ioctl(dev->device_handle, HIDIOCGFEATURE(length), data); + if (res < 0) + perror("ioctl (GFEATURE)"); + + + return res; +} + + +void HID_API_EXPORT hid_close(hid_device *dev) +{ + if (!dev) + return; + close(dev->device_handle); + free(dev); +} + + +int HID_API_EXPORT_CALL hid_get_manufacturer_string(hid_device *dev, wchar_t *string, size_t maxlen) +{ + return get_device_string(dev, DEVICE_STRING_MANUFACTURER, string, maxlen); +} + +int HID_API_EXPORT_CALL hid_get_product_string(hid_device *dev, wchar_t *string, size_t maxlen) +{ + return get_device_string(dev, DEVICE_STRING_PRODUCT, string, maxlen); +} + +int HID_API_EXPORT_CALL hid_get_serial_number_string(hid_device *dev, wchar_t *string, size_t maxlen) +{ + return get_device_string(dev, DEVICE_STRING_SERIAL, string, maxlen); +} + +int HID_API_EXPORT_CALL hid_get_indexed_string(hid_device *dev, int string_index, wchar_t *string, size_t maxlen) +{ + return -1; +} + + +HID_API_EXPORT const wchar_t * HID_API_CALL hid_error(hid_device *dev) +{ + return NULL; +} + diff --git a/src/survive_default_devices.c b/src/survive_default_devices.c new file mode 100644 index 0000000..6615f1e --- /dev/null +++ b/src/survive_default_devices.c @@ -0,0 +1,199 @@ +#include "survive_default_devices.h" +#include <jsmn.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> + +#include "json_helpers.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); +} + +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; +} + +int survive_load_htc_config_format(char *ct0conf, int len, SurviveObject *so) { + if (len == 0) + return -1; + + SurviveContext *ctx = so->ctx; + // 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; + } + } + } + + char fname[64]; + + 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; +} diff --git a/src/survive_default_devices.h b/src/survive_default_devices.h new file mode 100644 index 0000000..1fcca72 --- /dev/null +++ b/src/survive_default_devices.h @@ -0,0 +1,19 @@ +#ifndef _SURVIVE_DEFAULT_DEVICES_H +#define _SURVIVE_DEFAULT_DEVICES_H + +#include <survive.h> + +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); + +int survive_load_htc_config_format(char *ct0conf, int length, + SurviveObject *so); +#endif diff --git a/src/survive_internal.h b/src/survive_internal.h index e1a733d..86b119f 100644 --- a/src/survive_internal.h +++ b/src/survive_internal.h @@ -17,6 +17,7 @@ void * GetDriver( const char * name ); const char * GetDriverNameMatching( const char * prefix, int place ); void ListDrivers(); + #endif diff --git a/src/survive_playback.c b/src/survive_playback.c new file mode 100644 index 0000000..4b25b4c --- /dev/null +++ b/src/survive_playback.c @@ -0,0 +1,242 @@ +// All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL +// or LGPL licenses. + +#include <stdio.h> +#include <stdlib.h> +#include <survive.h> + +#include <string.h> +#include <sys/time.h> + +#include "survive_config.h" +#include "survive_default_devices.h" + +#include "redist/os_generic.h" + +struct SurvivePlaybackData { + SurviveContext *ctx; + const char *playback_dir; + FILE *playback_file; + int lineno; + + FLT time_factor; + double next_time_us; +}; +typedef struct SurvivePlaybackData SurvivePlaybackData; + +double timestamp_in_us() { + static double start_time_us = 0; + if (start_time_us == 0.) + start_time_us = OGGetAbsoluteTime(); + return OGGetAbsoluteTime() - 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; + + if (sscanf(line, "%lf", &driver->next_time_us) != 1) { + free(line); + return 0; + } + free(line); + line = 0; + } + + if (driver->next_time_us * driver->time_factor > 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; +} + +static 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) { + 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); + int read = fread(ct0conf, len, 1, f); + fclose(f); + ct0conf[len] = 0; + + printf("Loading config: %d\n", len); + return survive_load_htc_config_format(ct0conf, len, so); +} + +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; + sp->time_factor = + config_read_float(ctx->global_config_values, "PlaybackFactor", 1.); + + 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)) { + survive_add_object(ctx, hmd); + } + if (!LoadConfig(sp, wm0)) { + survive_add_object(ctx, wm0); + } + if (!LoadConfig(sp, wm1)) { + survive_add_object(ctx, wm1); + } + if (!LoadConfig(sp, tr0)) { + survive_add_object(ctx, tr0); + } + if (!LoadConfig(sp, ww0)) { + 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..288f8cb 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -23,6 +23,8 @@ #endif #include "json_helpers.h" +#include "survive_default_devices.h" +#include "survive_config.h" #ifdef HIDAPI #if defined(WINDOWS) || defined(WIN32) || defined (_WIN32) @@ -1647,171 +1649,22 @@ void survive_data_cb( SurviveUSBInterface * si ) -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; -} - static int LoadConfig( SurviveViveData * sv, SurviveObject * so, int devno, int iface, int extra_magic ) { 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 ); - if( len > 0 ) { - - //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; - } - } - } - } - else - { - //TODO: Cleanup any remaining USB stuff. - return 1; + 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 ); } - char fname[64]; - - 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; + return survive_load_htc_config_format(ct0conf, len, so); } @@ -1835,19 +1688,20 @@ 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); + const char* playback_dir = config_read_str(ctx->global_config_values, + "PlaybackDir", ""); + if(strlen(playback_dir) != 0) { + SV_INFO("Playback is active; disabling USB driver"); + return 0; + } + + int r; + 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 +1713,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,58 +1726,7 @@ 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; - printf( "Locs: %d\n", locs ); - if (hmd->sensor_locations ) - { - printf( "POSITIONS:\n" ); - for( i = 0; i < locs*3; i+=3 ) - { - printf( "%f %f %f\n", hmd->sensor_locations[i+0], hmd->sensor_locations[i+1], hmd->sensor_locations[i+2] ); - } - } - if( hmd->sensor_normals ) - { - printf( "NORMALS:\n" ); - for( i = 0; i < locs*3; i+=3 ) - { - printf( "%f %f %f\n", hmd->sensor_normals[i+0], hmd->sensor_normals[i+1], hmd->sensor_normals[i+2] ); - } - } -*/ - + //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 +1734,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: diff --git a/useful_files/81-vive.rules b/useful_files/81-vive.rules index ab38087..652b599 100644 --- a/useful_files/81-vive.rules +++ b/useful_files/81-vive.rules @@ -12,3 +12,13 @@ SUBSYSTEM=="usb", ATTRS{idVendor}=="114d", ATTRS{idProduct}=="8328", TAG+="uacce # HTC Mass Storage Node SUBSYSTEM=="usb", ATTRS{idVendor}=="114d", ATTRS{idProduct}=="8200", TAG+="uaccess" SUBSYSTEM=="usb", ATTRS{idVendor}=="114d", ATTRS{idProduct}=="8a12", TAG+="uaccess" + + +#libsurvive +SUBSYSTEM=="usb", ATTR{idVendor}=="0bb4", ATTR{idProduct}=="2c87", MODE="0666" # HTC HMD +SUBSYSTEM=="usb", ATTR{idVendor}=="28de", ATTR{idProduct}=="2000", MODE="0666" # Light input +SUBSYSTEM=="usb", ATTR{idVendor}=="28de", ATTR{idProduct}=="2101", MODE="0666" # Watchman + +#KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="0bb4", ATTR{idProduct}=="2c87", MODE="0666" # HTC +#KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="28de", ATTR{idProduct}=="2000", MODE="0666" # Valve +#KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="28de", ATTR{idProduct}=="2101", MODE="0666" # Valve |