aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--Makefile2
-rw-r--r--README.md2
-rw-r--r--data_recorder.c259
-rw-r--r--include/libsurvive/survive_types.h6
-rw-r--r--redist/hid-linux.c798
-rw-r--r--src/survive_default_devices.c199
-rw-r--r--src/survive_default_devices.h19
-rw-r--r--src/survive_internal.h1
-rw-r--r--src/survive_playback.c242
-rwxr-xr-xsrc/survive_vive.c277
-rw-r--r--useful_files/81-vive.rules10
11 files changed, 1454 insertions, 361 deletions
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/README.md b/README.md
index b587d3b..98f75c8 100644
--- a/README.md
+++ b/README.md
@@ -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