Add basic Oculus Rift CV1 support
authorThibaut Girka <thib@sitedethib.com>
Tue, 24 May 2016 21:27:44 +0200
changeset 32 cde110e18a4f
parent 31 db03d4ff40b6
child 33 760b375267af
Add basic Oculus Rift CV1 support
src/backends/rift.c
--- a/src/backends/rift.c	Mon Nov 10 09:33:12 2014 +0100
+++ b/src/backends/rift.c	Tue May 24 21:27:44 2016 +0200
@@ -40,7 +40,6 @@
 #include "quaternion.h"
 
 #define OVR_VID 0x2833
-#define RIFT_PID 1
 
 struct raw_sample {
   int32_t accel[3];
@@ -50,7 +49,7 @@
 
 struct raw_sensor_data {
   uint8_t sample_count;
-  uint16_t timestamp;
+  uint32_t timestamp;
   uint16_t command_id;
   int16_t temperature;
   struct raw_sample samples[3];
@@ -68,6 +67,7 @@
 
 struct device {
   hid_device *dev;
+  int32_t device_id;
   struct sensor_config sensor_config;
   struct raw_sensor_data raw_sensor_data; /* Last sensor data */
   unsigned int last_keepalive;
@@ -156,6 +156,59 @@
   return 0;
 }
 
+int rift_dk2_send_tracking(struct device *rift, uint8_t blink)
+{
+  /*
+     0: feature id (0x0c)
+     1-2: ???
+     3: pattern?
+     4: flags
+     5: ???
+     6-7: exposure in µs
+     8-9: period in µs
+     10-11: vsync offset
+     12: duty cycle?
+
+    Uninitialized CV1: 0c 00 00 00 02 00 7d 00 1b 41 00 00 00
+                       0c 00 00 02 03 00 7d 00 1b 41 00 00 00
+  */
+
+  uint8_t buffer[13] = { 0x0c };
+
+  if (hid_get_feature_report(rift->dev, buffer, 13) != 13)
+    return -1;
+
+  buffer[4] |= 1; /* enable tracking */
+
+  return hid_send_feature_report(rift->dev, buffer, 13);
+}
+
+int rift_dk2_send_display(struct device *rift, uint8_t low_persistance, uint8_t pixel_readback)
+{
+  /*
+     0: display config id (0x0d)
+     1-2: ???
+     3: brightness
+     4: flags1
+     5: flags2
+     6-7: ???
+     8-9: persistence
+     10-11: lighting_offset
+     12-13: pixel settle
+     14-15: total rows
+
+    Uninitialized CV1: 0d 00 00 00 00 04 00 00 11 01 00 00 64 00 c1 05
+  */
+
+  uint8_t buffer[16] = { 0x0d };
+
+  if (hid_get_feature_report(rift->dev, buffer, 16) != 16)
+    return -1;
+
+  /* TODO: stuff */
+  return hid_send_feature_report(rift->dev, buffer, 16);
+}
+
 int rift_update_sensors(struct device *rift, unsigned char *buffer, int size)
 {
   /* This is about parsing the input report provided in “buffer”...
@@ -216,7 +269,7 @@
   {
     sample = sensor_data->samples + i;
     decode_x21y21z21(buffer + 8 + 16*i, sample->accel);
-    decode_x21y21z21(buffer + 16 + 16*i, sample->gyro);
+    decode_x21y21z21(buffer + 8 + 16*i + 8, sample->gyro);
 
     /* gyro, accel and magnetometer values have to be multiplied by
        0.0001 to get a meaningful value */
@@ -243,6 +296,127 @@
 }
 
 
+int rift_update_sensors_2(struct device *rift, unsigned char *buffer, int size)
+{
+  /* This is about parsing the input report provided in “buffer”...
+     size: 64
+     All values are in little endian.
+     0: input report id (11)
+     1-2: last command id? Always zero
+     3: samples in this report
+       If more than two samples are reported, only two data points
+       are actually stored.
+     4-5: number of samples since start and before the current report
+     6-7: temperature (supposedly in 10⁻²°C?)
+     8-11: timestamp in µs
+     12-43: samples (up to 2)
+       for each sample:
+       0-7: accelerometer data packed as x21y21z21 (beware, sign extension needed!)
+       8-15: gyroscope data packed as x21y21z21, same thing
+       TODO: when more than two samples are reported, which is which?
+       XXX: When less than two samples are reported, the second sample is a
+            leftover
+     44-45: magnetometer x?
+     46-47: magnetometer y?
+     48-49: magnetometer z?
+
+     The following is according to pH5:
+       https://github.com/pH5/ouvrt/blob/master/src/rift-dk2.c#L384
+     50-51: Frame count?
+     52-55: Frame timestamp?
+     56: Frame id?
+     57: LED pattern phase?
+     58-59: exposure count?
+     60-63: exposure timestamp?
+
+Some sample subsequent packets:
+ID ZERO NB COUNT TEMP TIMESTMP Accel #1         Gyro #1          Accel #2         Gyro #2          MAG?         ????????????????????????????
+0b 0000 01 100e  4e0c 91027750 ff5e681eacc2bace 0001a800003fff96 ff40781edcc2bc4c ffff5ffff07ffe02 03009b080000 0000000000000000a81200ed7650
+0b 0000 01 110e  4e0c 7a067750 ff64681eacc2b7d0 0002f80012bfff16 ff40781edcc2bc4c ffff5ffff07ffe02 03009b080000 0000000000000000a81200ed7650
+0b 0000 01 120e  4e0c 640a7750 ff52701edcc2bace fffc58001abffe42 ff40781edcc2bc4c ffff5ffff07ffe02 03009b080000 0000000000000000a81200ed7650
+
+0b 0000 01 9e5b  540b 1521cb4c 0017f05b17bfbe2a 0004f8000fc00054 000bf85b777fbe2a 00045000057fff82 030073080000 00000000000000071108d70dcb4c
+0b 0000 01 9f5b  540b d024cb4c 000bf85a87ffbe2a 000157fff07fffec 000bf85b777fbe2a 00045000057fff82 030073080000 00000000000000071108d70dcb4c
+0b 0000 01 a05b  540b ba28cb4c 0011f05ae7bfbfa8 fffc5ffff2ffff42 000bf85b777fbe2a 00045000057fff82 030073080000 00000000000000071108d70dcb4c
+
+OR-ed over a long period of time with no set-up:
+0b 0000 1f ffff  ce0c ffff ff7f fffffffffffffffe fffffffffffffffe fffffffffffffffe fffffffffffffffe 7f009f080000 0000000000000000ff7f00ffff7f
+  */
+
+  printf("Debug: Sensors: ");
+  for (int i=0; i < size; i++) printf("%02x ", buffer[i]);
+  printf("\n");
+
+
+  struct raw_sensor_data *sensor_data = &rift->raw_sensor_data;
+  uint8_t nb_samples;
+  struct raw_sample *sample;
+  float accel[2];
+  float gyro[2];
+  float time_delta;
+  int i, j;
+
+  if (size != 64 || buffer[0] != 11)
+    return -1;
+
+  sensor_data->command_id = decode_int16(buffer, 1); //TODO: really?
+  sensor_data->sample_count = buffer[3];
+  sensor_data->temperature = decode_int16(buffer, 6);
+  sensor_data->timestamp = decode_int32(buffer, 8);
+
+  /* As I understand it, the timestamp is given in µs
+     and corresponds to the start of the sampling.
+     Only the last 2 samples are given. If there
+     are more than 2 samples, the first one is
+     an average (covering samples-1 ms).
+     TODO: Is that true?
+
+     This would mean the number of missed samples is:
+       (new_timestamp - old_timestamp) - old_count
+
+     An input report with n samples covers n ms,
+     with the first sample covering max(n - 1, 1) ms
+     and the subsequent ones 1 ms.
+  */
+
+  /* Note to implementer: beware of timestamp wrapping */
+
+  /* TODO: handle missing packets? */
+
+  time_delta = (max(sensor_data->sample_count, 2) - 1) * .001;
+  nb_samples = min(sensor_data->sample_count, 2);
+
+  for (i = 0; i < nb_samples; ++i)
+  {
+    sample = sensor_data->samples + i;
+    decode_x21y21z21(buffer + 12 + 16*i, sample->accel);
+    decode_x21y21z21(buffer + 12 + 16*i + 8, sample->gyro);
+
+    /* gyro, accel and magnetometer values have to be multiplied by
+       0.0001 to get a meaningful value */
+
+    /* Note to implementer: call sensor_fusion with gyro and accel values
+       for each sample, with the according time coverage of the sample, in
+       seconds */
+
+    for (j = 0; j < 2; ++j)
+    {
+      accel[j] = sample->accel[j] * .0001;
+      gyro[j] = sample->gyro[j] * .0001;
+    }
+    sensor_fusion(&(rift->sensor_fusion), gyro, accel, time_delta);
+
+    time_delta = .001;
+  }
+
+  sensor_data->mag_x = decode_int16(buffer, 44);
+  sensor_data->mag_y = decode_int16(buffer, 46);
+  sensor_data->mag_z = decode_int16(buffer, 48);
+
+  return 0;
+}
+
+
 /* Updating at each frame should be enough.
    Indeed, input reports are queued.
    The only catch is to not call rift_update frequently enough
@@ -268,6 +442,11 @@
         if ((ret = rift_update_sensors(rift, buffer, ret)) < 0)
           return ret;
         break;
+      case 11:
+        /* DK2/CV1 sensors update */
+        if ((ret = rift_update_sensors_2(rift, buffer, ret)) < 0)
+          return ret;
+        break;
       default:
         /* ??? */
         return -1;
@@ -346,6 +525,12 @@
 
 struct hmd *rift_open(void)
 {
+  static const int ids[] = {
+    0x0001, // DK1
+    0x0031, // CV1
+    0x2031, // Also CV1?
+  };
+
   struct hmd *hmd;
   struct device *rift;
   hid_device *dev = NULL;
@@ -356,7 +541,16 @@
   if ((rift = calloc(1, sizeof(struct device))) == NULL)
     goto rift_alloc_fail;
 
-  if ((dev = hid_open(OVR_VID, RIFT_PID, NULL)) == NULL)
+  for (int i = 0; i < sizeof (ids) / sizeof(ids[0]); i++)
+  {
+    dev = hid_open(OVR_VID, ids[i], NULL);
+    if (dev == NULL)
+      continue;
+    else
+      break;
+    rift->device_id = ids[i];
+  }
+  if (dev == NULL)
     goto hid_open_fail;
 
   sensor_fusion_init(&(rift->sensor_fusion));
@@ -370,6 +564,8 @@
   hmd->get_rotation = rift_get_rotation;
   hmd->get_position = NULL;
   rift_get_sensor_config(rift);
+//  rift_dk2_send_display(rift, 1, 1);
+//  rift_dk2_send_tracking(rift, 1);
 
   return hmd;