rift.c 12 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400
  1. /*
  2. * OpenHMD - Free and Open Source API and drivers for immersive technology.
  3. * Copyright (C) 2013 Fredrik Hultin.
  4. * Copyright (C) 2013 Jakob Bornecrantz.
  5. * Distributed under the Boost 1.0 licence, see LICENSE for full text.
  6. */
  7. /* Oculus Rift Driver - HID/USB Driver Implementation */
  8. #include <stdlib.h>
  9. #include <hidapi.h>
  10. #include <string.h>
  11. #include <stdio.h>
  12. #include <time.h>
  13. #include <assert.h>
  14. #include "rift.h"
  15. #define TICK_LEN (1.0f / 1000.0f) // 1000 Hz ticks
  16. #define KEEP_ALIVE_VALUE (10 * 1000)
  17. #define SETFLAG(_s, _flag, _val) (_s) = ((_s) & ~(_flag)) | ((_val) ? (_flag) : 0)
  18. typedef struct {
  19. ohmd_device base;
  20. hid_device* handle;
  21. pkt_sensor_range sensor_range;
  22. pkt_sensor_display_info display_info;
  23. rift_coordinate_frame coordinate_frame, hw_coordinate_frame;
  24. pkt_sensor_config sensor_config;
  25. pkt_tracker_sensor sensor;
  26. double last_keep_alive;
  27. fusion sensor_fusion;
  28. vec3f raw_mag, raw_accel, raw_gyro;
  29. // These values are derived from the display_info struct and
  30. // from user provided values.
  31. struct {
  32. float idp; // inter-pupillary distance, user provided.
  33. float znear; // depth near value, user provided.
  34. float zfar; // depth far value, user provided.
  35. float proj_offset; // lens offset on screen
  36. mat4x4f proj_base; // base projection matrix
  37. mat4x4f proj_left; // adjusted projection matrix for left screen
  38. mat4x4f proj_right; // adjusted projection matrix for right screen
  39. float full_ratio; // screen ratio for the entire device
  40. float stereo_fov; // horizontal fov for one sub screen
  41. float stereo_ratio; // screen ratio for one sub screen
  42. } calc_values;
  43. } rift_priv;
  44. static rift_priv* rift_priv_get(ohmd_device* device)
  45. {
  46. return (rift_priv*)device;
  47. }
  48. static int get_feature_report(rift_priv* priv, rift_sensor_feature_cmd cmd, unsigned char* buf)
  49. {
  50. memset(buf, 0, FEATURE_BUFFER_SIZE);
  51. buf[0] = (unsigned char)cmd;
  52. return hid_get_feature_report(priv->handle, buf, FEATURE_BUFFER_SIZE);
  53. }
  54. static int send_feature_report(rift_priv* priv, const unsigned char *data, size_t length)
  55. {
  56. return hid_send_feature_report(priv->handle, data, length);
  57. }
  58. static void set_coordinate_frame(rift_priv* priv, rift_coordinate_frame coordframe)
  59. {
  60. priv->coordinate_frame = coordframe;
  61. // set the RIFT_SCF_SENSOR_COORDINATES in the sensor config to match whether coordframe is hmd or sensor
  62. SETFLAG(priv->sensor_config.flags, RIFT_SCF_SENSOR_COORDINATES, coordframe == RIFT_CF_SENSOR);
  63. // encode send the new config to the Rift
  64. unsigned char buf[FEATURE_BUFFER_SIZE];
  65. int size = encode_sensor_config(buf, &priv->sensor_config);
  66. if(send_feature_report(priv, buf, size) == -1){
  67. ohmd_set_error(priv->base.ctx, "send_feature_report failed in set_coordinate frame");
  68. return;
  69. }
  70. // read the state again, set the hw_coordinate_frame to match what
  71. // the hardware actually is set to just incase it doesn't stick.
  72. size = get_feature_report(priv, RIFT_CMD_SENSOR_CONFIG, buf);
  73. if(size <= 0){
  74. LOGW("could not set coordinate frame");
  75. priv->hw_coordinate_frame = RIFT_CF_HMD;
  76. return;
  77. }
  78. decode_sensor_config(&priv->sensor_config, buf, size);
  79. priv->hw_coordinate_frame = (priv->sensor_config.flags & RIFT_SCF_SENSOR_COORDINATES) ? RIFT_CF_SENSOR : RIFT_CF_HMD;
  80. if(priv->hw_coordinate_frame != coordframe) {
  81. LOGW("coordinate frame didn't stick");
  82. }
  83. }
  84. static void handle_tracker_sensor_msg(rift_priv* priv, unsigned char* buffer, int size)
  85. {
  86. if(!decode_tracker_sensor_msg(&priv->sensor, buffer, size)){
  87. LOGE("couldn't decode tracker sensor message");
  88. }
  89. pkt_tracker_sensor* s = &priv->sensor;
  90. dump_packet_tracker_sensor(s);
  91. // TODO handle missed samples etc.
  92. float dt = s->num_samples > 3 ? (s->num_samples - 2) * TICK_LEN : TICK_LEN;
  93. int32_t mag32[] = { s->mag[0], s->mag[1], s->mag[2] };
  94. vec3f_from_rift_vec(mag32, &priv->raw_mag);
  95. for(int i = 0; i < OHMD_MIN(s->num_samples, 3); i++){
  96. vec3f_from_rift_vec(s->samples[i].accel, &priv->raw_accel);
  97. vec3f_from_rift_vec(s->samples[i].gyro, &priv->raw_gyro);
  98. ofusion_update(&priv->sensor_fusion, dt, &priv->raw_gyro, &priv->raw_accel, &priv->raw_mag);
  99. // reset dt to tick_len for the last samples if there were more than one sample
  100. dt = TICK_LEN;
  101. }
  102. }
  103. static void update_device(ohmd_device* device)
  104. {
  105. rift_priv* priv = rift_priv_get(device);
  106. unsigned char buffer[FEATURE_BUFFER_SIZE];
  107. // Handle keep alive messages
  108. double t = ohmd_get_tick();
  109. if(t - priv->last_keep_alive >= (double)priv->sensor_config.keep_alive_interval / 1000.0 - .2){
  110. // send keep alive message
  111. pkt_keep_alive keep_alive = { 0, priv->sensor_config.keep_alive_interval };
  112. int ka_size = encode_keep_alive(buffer, &keep_alive);
  113. send_feature_report(priv, buffer, ka_size);
  114. // Update the time of the last keep alive we have sent.
  115. priv->last_keep_alive = t;
  116. }
  117. // Read all the messages from the device.
  118. while(true){
  119. int size = hid_read(priv->handle, buffer, FEATURE_BUFFER_SIZE);
  120. if(size < 0){
  121. LOGE("error reading from device");
  122. return;
  123. } else if(size == 0) {
  124. return; // No more messages, return.
  125. }
  126. // currently the only message type the hardware supports (I think)
  127. if(buffer[0] == RIFT_IRQ_SENSORS){
  128. handle_tracker_sensor_msg(priv, buffer, size);
  129. }else{
  130. LOGE("unknown message type: %u", buffer[0]);
  131. }
  132. }
  133. }
  134. static void calc_derived_values(rift_priv *priv)
  135. {
  136. priv->calc_values.idp = 0.061f; // TODO settable.
  137. priv->calc_values.znear = 0.1f; // TODO settable.
  138. priv->calc_values.zfar = 1000.0f; // TODO settable.
  139. priv->calc_values.stereo_fov = DEG_TO_RAD(125.5144f); // TODO calculate.
  140. // Calculate the screen ratio of each subscreen.
  141. float full_ratio = (float)priv->display_info.h_resolution /
  142. (float)priv->display_info.v_resolution;
  143. float ratio = full_ratio / 2.0f;
  144. priv->calc_values.stereo_ratio = ratio;
  145. // Calculate where the lens is on each screen,
  146. // and with the given value offset the projection matrix.
  147. float screen_center = priv->display_info.h_screen_size / 4.0f;
  148. float lens_shift = screen_center - priv->display_info.lens_separation / 2.0f;
  149. float proj_offset = 4.0f * lens_shift / priv->display_info.h_screen_size;
  150. priv->calc_values.proj_offset = proj_offset;
  151. // Setup the base projection matrix. Each eye mostly have the
  152. // same projection matrix with the exception of the offset.
  153. omat4x4f_init_perspective(&priv->calc_values.proj_base,
  154. priv->calc_values.stereo_fov,
  155. priv->calc_values.stereo_ratio,
  156. priv->calc_values.znear,
  157. priv->calc_values.zfar);
  158. // Setup the two adjusted projection matricies. Each is setup to deal
  159. // with the fact that the lens is not in the center of the screen.
  160. // These matrices only change of the hardware changes, so static.
  161. mat4x4f translate;
  162. omat4x4f_init_translate(&translate, proj_offset, 0, 0);
  163. omat4x4f_mult(&translate,
  164. &priv->calc_values.proj_base,
  165. &priv->calc_values.proj_left);
  166. omat4x4f_init_translate(&translate, -proj_offset, 0, 0);
  167. omat4x4f_mult(&translate,
  168. &priv->calc_values.proj_base,
  169. &priv->calc_values.proj_right);
  170. }
  171. static int getf(ohmd_device* device, ohmd_float_value type, float* out)
  172. {
  173. rift_priv* priv = rift_priv_get(device);
  174. switch(type){
  175. case OHMD_ROTATION_QUAT: {
  176. *(quatf*)out = priv->sensor_fusion.orient;
  177. break;
  178. }
  179. case OHMD_MAT4X4_LEFT_EYE_GL_MODELVIEW: {
  180. vec3f point = {{0, 0, 0}};
  181. mat4x4f orient, world_shift, result;
  182. omat4x4f_init_look_at(&orient, &priv->sensor_fusion.orient, &point);
  183. omat4x4f_init_translate(&world_shift, +(priv->calc_values.idp / 2.0f), 0, 0);
  184. omat4x4f_mult(&world_shift, &orient, &result);
  185. omat4x4f_transpose(&result, (mat4x4f*)out);
  186. break;
  187. }
  188. case OHMD_MAT4X4_RIGHT_EYE_GL_MODELVIEW: {
  189. vec3f point = {{0, 0, 0}};
  190. mat4x4f orient, world_shift, result;
  191. omat4x4f_init_look_at(&orient, &priv->sensor_fusion.orient, &point);
  192. omat4x4f_init_translate(&world_shift, -(priv->calc_values.idp / 2.0f), 0, 0);
  193. omat4x4f_mult(&world_shift, &orient, &result);
  194. omat4x4f_transpose(&result, (mat4x4f*)out);
  195. break;
  196. }
  197. case OHMD_MAT4X4_LEFT_EYE_GL_PROJECTION: {
  198. omat4x4f_transpose(&priv->calc_values.proj_left, (mat4x4f*)out);
  199. break;
  200. }
  201. case OHMD_MAT4X4_RIGHT_EYE_GL_PROJECTION: {
  202. omat4x4f_transpose(&priv->calc_values.proj_right, (mat4x4f*)out);
  203. break;
  204. }
  205. default:
  206. ohmd_set_error(priv->base.ctx, "invalid type given to getf (%d)", type);
  207. return -1;
  208. break;
  209. }
  210. return 0;
  211. }
  212. static void close_device(ohmd_device* device)
  213. {
  214. LOGD("closing device");
  215. rift_priv* priv = rift_priv_get(device);
  216. hid_close(priv->handle);
  217. free(priv);
  218. }
  219. static ohmd_device* open_device(ohmd_driver* driver, ohmd_device_desc* desc)
  220. {
  221. rift_priv* priv = ohmd_alloc(driver->ctx, sizeof(rift_priv));
  222. if(!priv)
  223. goto cleanup;
  224. priv->base.ctx = driver->ctx;
  225. // Open the HID device
  226. priv->handle = hid_open_path(desc->path);
  227. if(!priv->handle)
  228. goto cleanup;
  229. if(hid_set_nonblocking(priv->handle, 1) == -1){
  230. ohmd_set_error(driver->ctx, "failed to set non-blocking on device");
  231. goto cleanup;
  232. }
  233. unsigned char buf[FEATURE_BUFFER_SIZE];
  234. int size;
  235. // Read and decode the sensor range
  236. size = get_feature_report(priv, RIFT_CMD_RANGE, buf);
  237. decode_sensor_range(&priv->sensor_range, buf, size);
  238. dump_packet_sensor_range(&priv->sensor_range);
  239. // Read and decode display information
  240. size = get_feature_report(priv, RIFT_CMD_DISPLAY_INFO, buf);
  241. decode_sensor_display_info(&priv->display_info, buf, size);
  242. dump_packet_sensor_display_info(&priv->display_info);
  243. // Read and decode the sensor config
  244. size = get_feature_report(priv, RIFT_CMD_SENSOR_CONFIG, buf);
  245. decode_sensor_config(&priv->sensor_config, buf, size);
  246. dump_packet_sensor_config(&priv->sensor_config);
  247. // if the sensor has display info data, use HMD coordinate frame
  248. priv->coordinate_frame = priv->display_info.distortion_type != RIFT_DT_NONE ? RIFT_CF_HMD : RIFT_CF_SENSOR;
  249. // apply sensor config
  250. set_coordinate_frame(priv, priv->coordinate_frame);
  251. // set keep alive interval to n seconds
  252. pkt_keep_alive keep_alive = { 0, KEEP_ALIVE_VALUE };
  253. size = encode_keep_alive(buf, &keep_alive);
  254. send_feature_report(priv, buf, size);
  255. // Update the time of the last keep alive we have sent.
  256. priv->last_keep_alive = ohmd_get_tick();
  257. // update sensor settings with new keep alive value
  258. // (which will have been ignored in favor of the default 1000 ms one)
  259. size = get_feature_report(priv, RIFT_CMD_SENSOR_CONFIG, buf);
  260. decode_sensor_config(&priv->sensor_config, buf, size);
  261. dump_packet_sensor_config(&priv->sensor_config);
  262. calc_derived_values(priv);
  263. priv->base.update = update_device;
  264. priv->base.close = close_device;
  265. priv->base.getf = getf;
  266. ofusion_init(&priv->sensor_fusion);
  267. return &priv->base;
  268. cleanup:
  269. if(priv)
  270. free(priv);
  271. return NULL;
  272. }
  273. static void get_device_list(ohmd_driver* driver, ohmd_device_list* list)
  274. {
  275. // enumerate HID devices and add any Rifts found to the device list
  276. #define OCULUS_VR_INC_ID 0x2833
  277. #define RIFT_DEVKIT_ID 0x0001
  278. struct hid_device_info* devs = hid_enumerate(OCULUS_VR_INC_ID, RIFT_DEVKIT_ID);
  279. struct hid_device_info* cur_dev = devs;
  280. if(devs == NULL)
  281. return;
  282. while (cur_dev) {
  283. ohmd_device_desc* desc = &list->devices[list->num_devices++];
  284. strcpy(desc->driver, "OpenHMD Rift Driver");
  285. strcpy(desc->vendor, "Oculus VR, Inc.");
  286. strcpy(desc->product, "Rift (Devkit)");
  287. strcpy(desc->path, cur_dev->path);
  288. desc->driver_ptr = driver;
  289. cur_dev = cur_dev->next;
  290. }
  291. hid_free_enumeration(devs);
  292. }
  293. static void destroy_driver(ohmd_driver* drv)
  294. {
  295. LOGD("shutting down driver");
  296. hid_exit();
  297. free(drv);
  298. }
  299. ohmd_driver* ohmd_create_oculus_rift_drv(ohmd_context* ctx)
  300. {
  301. ohmd_driver* drv = ohmd_alloc(ctx, sizeof(ohmd_driver));
  302. if(drv == NULL)
  303. return NULL;
  304. drv->get_device_list = get_device_list;
  305. drv->open_device = open_device;
  306. drv->ctx = ctx;
  307. drv->get_device_list = get_device_list;
  308. drv->open_device = open_device;
  309. drv->destroy = destroy_driver;
  310. return drv;
  311. }