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. @@
_______________________________________________
Bf-blender-cvs mailing list
[email protected]
https://lists.blender.org/mailman/listinfo/bf-blender-cvs
