[Bf-blender-cvs] [e7670e9] HMD_viewport: - Added initial HTC Vive support, still uses complimentary sensor fusion, could drift a bit

TheOnlyJoey noreply at git.blender.org
Sat Oct 29 16:58:27 CEST 2016


Commit: e7670e9bb08194bf2008672b63ffdbf60303e22f
Author: TheOnlyJoey
Date:   Sat Oct 29 16:21:56 2016 +0200
Branches: HMD_viewport
https://developer.blender.org/rBe7670e9bb08194bf2008672b63ffdbf60303e22f

- Added initial HTC Vive support, still uses complimentary sensor fusion, could drift a bit

===================================================================

M	extern/openhmd/CMakeLists.txt
A	extern/openhmd/src/drv_htc_vive/magic.h
A	extern/openhmd/src/drv_htc_vive/packet.c
A	extern/openhmd/src/drv_htc_vive/vive.c
A	extern/openhmd/src/drv_htc_vive/vive.h
M	extern/openhmd/src/openhmd.c
M	extern/openhmd/src/openhmdi.h

===================================================================

diff --git a/extern/openhmd/CMakeLists.txt b/extern/openhmd/CMakeLists.txt
index 32dfc28..398d9c9 100644
--- a/extern/openhmd/CMakeLists.txt
+++ b/extern/openhmd/CMakeLists.txt
@@ -38,6 +38,7 @@ set(SRC
 # TODO: Either remove this or move to main CMakeLists.txt
 OPTION(OPENHMD_DRIVER_OCULUS_RIFT "Oculus Rift DK1, DK2 and CV1" ON)
 OPTION(OPENHMD_DRIVER_DEEPOON "Deepoon E2 support" ON)
+OPTION(OPENHMD_DRIVER_DEEPOON "HTC Vive support" ON)
 OPTION(OPENHMD_DRIVER_EXTERNAL "External sensor driver" OFF)
 OPTION(OPENHMD_DRIVER_ANDROID "General Android driver" OFF)
 
@@ -68,6 +69,17 @@ if(OPENHMD_DRIVER_DEEPOON)
 	list(APPEND INC_SYS ${HIDAPI_INCLUDE_DIRS})
 endif()
 
+if(OPENHMD_HTC_VIVE)
+	list(APPEND SRC
+		src/drv_htc_vive/vive.c
+		src/drv_htc_vive/packet.c
+	)
+	add_definitions(-DDRIVER_HTC_VIVE)
+
+	find_package(HIDAPI REQUIRED)
+	list(APPEND INC_SYS ${HIDAPI_INCLUDE_DIRS})
+endif()
+
 if(OPENHMD_DRIVER_EXTERNAL)
 	list(APPEND SRC
 		src/drv_external/external.c
diff --git a/extern/openhmd/src/drv_htc_vive/magic.h b/extern/openhmd/src/drv_htc_vive/magic.h
new file mode 100644
index 0000000..3464dd6
--- /dev/null
+++ b/extern/openhmd/src/drv_htc_vive/magic.h
@@ -0,0 +1,27 @@
+static const unsigned char vive_magic_power_on[64] = {
+	0x04, 0x78, 0x29, 0x38, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x01,
+	0xa8, 0x0d, 0x76, 0x00, 0x40, 0xfc, 0x01, 0x05, 0xfa, 0xec, 0xd1, 0x6d, 0x00,
+	0x00, 0x6c, 0x00, 0x00, 0x00, 0x00, 0x00, 0xa8, 0x0d, 0x76, 0x00, 0x68, 0xfc,
+	0x01, 0x05, 0x2c, 0xb0, 0x2e, 0x65, 0x7a, 0x0d, 0x76, 0x00, 0x68, 0x54, 0x72,
+	0x00, 0x18, 0x54, 0x72, 0x00, 0x00, 0x6a, 0x72, 0x00, 0x00, 0x00, 0x00,
+};
+
+static const unsigned char vive_magic_power_off1[64] = {
+	0x04, 0x78, 0x29, 0x38, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00,
+	0x30, 0x05, 0x77, 0x00, 0x30, 0x05, 0x77, 0x00, 0x6c, 0x4d, 0x37, 0x65, 0x40,
+	0xf9, 0x33, 0x00, 0x04, 0xf8, 0xa3, 0x04, 0x04, 0x00, 0x00, 0x00, 0x70, 0xb0,
+	0x72, 0x00, 0xf4, 0xf7, 0xa3, 0x04, 0x7c, 0xf8, 0x33, 0x00, 0x0c, 0xf8, 0xa3,
+	0x04, 0x0a, 0x6e, 0x29, 0x65, 0x24, 0xf9, 0x33, 0x00, 0x00, 0x00, 0x00,
+};
+
+static const unsigned char vive_magic_power_off2[64] = {
+	0x04, 0x78, 0x29, 0x38, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00,
+	0x30, 0x05, 0x77, 0x00, 0xe4, 0xf7, 0x33, 0x00, 0xe4, 0xf7, 0x33, 0x00, 0x60,
+	0x6e, 0x72, 0x00, 0xb4, 0xf7, 0x33, 0x00, 0x04, 0x00, 0x00, 0x00, 0x70, 0xb0,
+	0x72, 0x00, 0x90, 0xf7, 0x33, 0x00, 0x7c, 0xf8, 0x33, 0x00, 0xd0, 0xf7, 0x33,
+	0x00, 0x3c, 0x68, 0x29, 0x65, 0x24, 0xf9, 0x33, 0x00, 0x00, 0x00, 0x00,
+};
+
+static const unsigned char vive_magic_enable_lighthouse[5] = {
+	0x04
+};
diff --git a/extern/openhmd/src/drv_htc_vive/packet.c b/extern/openhmd/src/drv_htc_vive/packet.c
new file mode 100644
index 0000000..2192948
--- /dev/null
+++ b/extern/openhmd/src/drv_htc_vive/packet.c
@@ -0,0 +1,62 @@
+#include "vive.h"
+
+/* Suppress the warnings for this include, since we don't care about them for external dependencies
+/* Requires at least GCC 4.6 or higher
+*/
+
+inline static uint8_t read8(const unsigned char** buffer)
+{
+	uint8_t ret = **buffer;
+	*buffer += 1;
+	return ret;
+}
+
+inline static int16_t read16(const unsigned char** buffer)
+{
+	int16_t ret = **buffer | (*(*buffer + 1) << 8);
+	*buffer += 2;
+	return ret;
+}
+
+inline static uint32_t read32(const unsigned char** buffer)
+{
+	uint32_t ret = **buffer | (*(*buffer + 1) << 8) | (*(*buffer + 2) << 16) | (*(*buffer + 3) << 24);
+	*buffer += 4;
+	return ret;
+}
+
+bool vive_decode_sensor_packet(vive_sensor_packet* pkt, const unsigned char* buffer, int size)
+{
+	if(size != 52){
+		LOGE("invalid vive sensor packet size (expected 52 but got %d)", size);
+		return false;
+	}
+
+	pkt->report_id = read8(&buffer);
+
+	for(int j = 0; j < 3; j++){
+		// acceleration
+		for(int i = 0; i < 3; i++){
+			pkt->samples[j].acc[i] = read16(&buffer);
+		}
+
+		// rotation
+		for(int i = 0; i < 3; i++){
+			pkt->samples[j].rot[i] = read16(&buffer);
+		}
+
+		pkt->samples[j].time_ticks = read32(&buffer);
+		pkt->samples[j].seq = read8(&buffer);
+	}
+
+	return true;
+}
+//TODO: Implement config packet decoding.
+bool vive_decode_config_packet(vive_config_packet* pkt, const unsigned char* buffer, int size)
+{
+	if(size != 64){
+		LOGE("invalid vive sensor packet size (expected 64 but got %d)", size);
+		return false;
+	}
+	return false;
+}
diff --git a/extern/openhmd/src/drv_htc_vive/vive.c b/extern/openhmd/src/drv_htc_vive/vive.c
new file mode 100644
index 0000000..39e0356
--- /dev/null
+++ b/extern/openhmd/src/drv_htc_vive/vive.c
@@ -0,0 +1,405 @@
+/*
+ * OpenHMD - Free and Open Source API and drivers for immersive technology.
+ * Copyright (C) 2013 Fredrik Hultin.
+ * Copyright (C) 2013 Jakob Bornecrantz.
+ * Distributed under the Boost 1.0 licence, see LICENSE for full text.
+ */
+
+/* HTC Vive Driver */
+
+#define FEATURE_BUFFER_SIZE 256
+
+#define HTC_ID                   0x0bb4
+#define VIVE_HMD                 0x2c87
+
+#define VALVE_ID                 0x28de
+#define VIVE_WATCHMAN_DONGLE     0x2101
+#define VIVE_LIGHTHOUSE_FPGA_RX  0x2000
+
+#define VIVE_TIME_DIV 48000000.0f
+
+#include <string.h>
+#include <wchar.h>
+#include <hidapi.h>
+#include <assert.h>
+#include <limits.h>
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "vive.h"
+#include "openhmdi.h"
+
+typedef struct {
+	ohmd_device base;
+
+	hid_device* hmd_handle;
+	hid_device* imu_handle;
+	fusion sensor_fusion;
+	vec3f raw_accel, raw_gyro;
+	uint32_t last_ticks;
+	uint8_t last_seq;
+
+	vec3f gyro_error;
+	filter_queue gyro_q;
+} vive_priv;
+
+void vec3f_from_vive_vec_accel(const int16_t* smp, vec3f* out_vec)
+{
+	float gravity = 9.81f;
+	float scaler = 4.0f * gravity / 32768.0f;
+
+	out_vec->x = (float)smp[0] * scaler;
+	out_vec->y = (float)smp[1] * scaler * -1;
+	out_vec->z = (float)smp[2] * scaler * -1;
+}
+
+void vec3f_from_vive_vec_gyro(const int16_t* smp, vec3f* out_vec)
+{
+	float scaler = 8.7f / 32768.0f;
+	out_vec->x = (float)smp[0] * scaler;
+	out_vec->y = (float)smp[1] * scaler * -1;
+	out_vec->z = (float)smp[2] * scaler * -1;
+}
+
+static bool process_error(vive_priv* priv)
+{
+	if(priv->gyro_q.at >= priv->gyro_q.size - 1)
+		return true;
+
+	ofq_add(&priv->gyro_q, &priv->raw_gyro);
+
+	if(priv->gyro_q.at >= priv->gyro_q.size - 1){
+		ofq_get_mean(&priv->gyro_q, &priv->gyro_error);
+		printf("gyro error: %f, %f, %f\n", priv->gyro_error.x, priv->gyro_error.y, priv->gyro_error.z);
+	}
+
+	return false;
+}
+
+vive_sensor_sample* get_next_sample(vive_sensor_packet* pkt, int last_seq)
+{
+	int diff[3];
+
+	for(int i = 0; i < 3; i++)
+	{
+		diff[i] = (int)pkt->samples[i].seq - last_seq;
+
+		if(diff[i] < -128){
+			diff[i] += 256;
+		}
+	}
+
+	int closest_diff = INT_MAX;
+	int closest_idx = -1;
+
+	for(int i = 0; i < 3; i++)
+	{
+		if(diff[i] < closest_diff && diff[i] > 0 && diff[i] < 128){
+			closest_diff = diff[i];
+			closest_idx = i;
+		}
+	}
+
+	if(closest_idx != -1)
+		return pkt->samples + closest_idx;
+
+	return NULL;
+}
+
+static void update_device(ohmd_device* device)
+{
+	vive_priv* priv = (vive_priv*)device;
+
+	int size = 0;
+	unsigned char buffer[FEATURE_BUFFER_SIZE];
+
+	while((size = hid_read(priv->imu_handle, buffer, FEATURE_BUFFER_SIZE)) > 0){
+		if(buffer[0] == VIVE_IRQ_SENSORS){
+			vive_sensor_packet pkt;
+			vive_decode_sensor_packet(&pkt, buffer, size);
+
+			vive_sensor_sample* smp = NULL;
+
+			while((smp = get_next_sample(&pkt, priv->last_seq)) != NULL)
+			{
+				if(priv->last_ticks == 0)
+					priv->last_ticks = smp->time_ticks;
+
+				uint32_t t1, t2;
+				t1 = smp->time_ticks;
+				t2 = priv->last_ticks;
+
+				float dt = (t1 - t2) / VIVE_TIME_DIV;
+
+				priv->last_ticks = smp->time_ticks;
+
+				vec3f_from_vive_vec_accel(smp->acc, &priv->raw_accel);
+				vec3f_from_vive_vec_gyro(smp->rot, &priv->raw_gyro);
+
+				if(process_error(priv)){
+					vec3f mag = {{0.0f, 0.0f, 0.0f}};
+					vec3f gyro;
+					ovec3f_subtract(&priv->raw_gyro, &priv->gyro_error, &gyro);
+
+					ofusion_update(&priv->sensor_fusion, dt, &gyro, &priv->raw_accel, &mag);
+				}
+
+				priv->last_seq = smp->seq;
+			}
+		}else{
+			LOGE("unknown message type: %u", buffer[0]);
+		}
+	}
+
+	if(size < 0){
+		LOGE("error reading from device");
+	}
+}
+
+static int getf(ohmd_device* device, ohmd_float_value type, float* out)
+{
+	vive_priv* priv = (vive_priv*)device;
+
+	switch(type){
+	case OHMD_ROTATION_QUAT:
+		*(quatf*)out = priv->sensor_fusion.orient;
+		break;
+
+	case OHMD_POSITION_VECTOR:
+		out[0] = out[1] = out[2] = 0;
+		break;
+
+	case OHMD_DISTORTION_K:
+		// TODO this should be set to the equivalent of no distortion
+		memset(out, 0, sizeof(float) * 6);
+		break;
+
+	default:
+		ohmd_set_error(priv->base.ctx, "invalid type given to getf (%ud)", type);
+		return -1;
+		break;
+	}
+
+	return 0;
+}
+
+static void close_device(ohmd_device* device)
+{
+	int hret = 0;
+	vive_priv* priv = (vive_priv*)device;
+
+	LOGD("closing HTC Vive device");
+
+	// turn the display off
+	hret = hid_send_feature_report(priv->hmd_handle, vive_magic_power_off1, sizeof(vive_magic_power_off1));
+	printf("power off magic 1: %d\n", hret);
+
+	hret = hid_send_feature_report(priv->hmd_handle, vive_magic_power_off2, sizeof(vive_magic_power_off2));
+	printf("power off magic 2: %d\n", hret);
+
+	hid_close(priv->hmd_handle);
+	hid_close(priv->imu_handle);
+
+	free(device);
+}
+
+#if 0
+static void dump_indexed_string(hid_device* device, int index)
+{
+	wchar_t wbuffer[512] = {0};
+	char buffer[1024] = {0};
+
+	int hret = hid_get_indexed_string(device, index, wbuffer, 511);
+
+	if(hret == 0){
+		wcstombs(buffer, wbuffer, sizeof(buffer));
+		printf("indexed string 0x%02x: '%s'\n", index, buffer);
+	}
+}
+#endif
+
+static void dump_info_string(int (*fun)(hid_device*, wchar_t*, size_t), const char* what, hid_device* device)
+{
+	wchar_t wbuffer[512] = {0};
+	char buffer[1024] = {0};
+
+	int hret = fun(device, wbuffer, 511);
+
+	if(hret == 0){
+		wcstombs(buffer, wbuffer, sizeof(buffer));
+		printf("%s: '%s'\n", what, buffer);
+	}
+}
+
+#if 0
+static void dumpbin(const char* label, const unsigned char* data, int length)
+{
+	printf("%s:\n", label);
+	for(int i = 0; i < length; i++){
+		printf("%02x ", data[i]);
+		if((i % 16) == 15)
+			printf

@@ Diff output truncated at 10240 characters. @@




More information about the Bf-blender-cvs mailing list