Added getting of the LED positions (for DK2, should work for CV1 as well), sync works in OpenHMD-RiftPlayground

src/drv_oculus_rift/packet.c

@@ -10,6 +10,7 @@
 #include <stdio.h>
 #include "rift.h"
 
+#define SKIP8 (buffer++)
 #define SKIP_CMD (buffer++)
 #define READ8 *(buffer++);
 #define READ16 *buffer | (*(buffer + 1) << 8); buffer += 2;
@@ -21,6 +22,32 @@
 #define WRITE16(_val) WRITE8((_val) & 0xff); WRITE8(((_val) >> 8) & 0xff);
 #define WRITE32(_val) WRITE16((_val) & 0xffff) *buffer; WRITE16(((_val) >> 16) & 0xffff);
 
+bool decode_position_info(pkt_position_info* p, const unsigned char* buffer, int size)
+{
+	if(size != 30) {
+		LOGE("invalid packet size (expected 30 but got %d)", size);
+		return false;
+	}
+
+	SKIP_CMD;
+	SKIP8;
+	SKIP8;
+	p->flags = READ8;
+	p->pos_x = READ32;
+	p->pos_y = READ32;
+	p->pos_z = READ32;
+	p->dir_x = READ16;
+	p->dir_y = READ16;
+	p->dir_z = READ16;
+	SKIP8;
+	SKIP8;
+	p->index = READ8;
+	SKIP8;
+	p->num = READ8;
+
+	return true;
+}
+
 bool decode_sensor_range(pkt_sensor_range* range, const unsigned char* buffer, int size)
 {
 	if(!(size == 8 || size == 9)){

src/drv_oculus_rift/rift.c

@@ -33,6 +33,12 @@ typedef struct {
 	double last_keep_alive;
 	fusion sensor_fusion;
 	vec3f raw_mag, raw_accel, raw_gyro;
+
+	struct {
+		vec3f pos;
+	} imu;
+
+	rift_led *leds;
 } rift_priv;
 
 typedef enum {
@@ -295,6 +301,38 @@ static ohmd_device* open_device(ohmd_driver* driver, ohmd_device_desc* desc)
 		hid_write(priv->handle, rift_enable_leds_dk2, sizeof(rift_enable_leds_dk2));
 	}
 
+	pkt_position_info pos;
+	int first_index = -1;
+
+	//Get LED positions
+	while (true) {
+		size = get_feature_report(priv, RIFT_CMD_POSITION_INFO, buf);
+		if (size <= 0 || !decode_position_info(&pos, buf, size) ||
+		    first_index == pos.index) {
+			break;
+		}
+
+		if (first_index < 0) {
+			first_index = pos.index;
+			priv->leds = calloc(pos.num, sizeof(rift_led));
+		}
+
+		if (pos.flags == 1) { //reports 0's
+			priv->imu.pos.x = (float)pos.pos_x;
+			priv->imu.pos.y = (float)pos.pos_y;
+			priv->imu.pos.z = (float)pos.pos_z;
+		} else if (pos.flags == 2) {
+			rift_led *led = &priv->leds[pos.index];
+			led->pos.x = (float)pos.pos_x;
+			led->pos.y = (float)pos.pos_y;
+			led->pos.z = (float)pos.pos_z;
+			led->dir.x = (float)pos.dir_x;
+			led->dir.y = (float)pos.dir_y;
+			led->dir.z = (float)pos.dir_z;
+			ovec3f_normalize_me(&led->dir);
+		}
+	}
+
 	// set keep alive interval to n seconds
 	pkt_keep_alive keep_alive = { 0, KEEP_ALIVE_VALUE };
 	size = encode_keep_alive(buf, &keep_alive);

src/drv_oculus_rift/rift.h

@@ -19,7 +19,8 @@ typedef enum {
 	RIFT_CMD_RANGE = 4,
 	RIFT_CMD_KEEP_ALIVE = 8,
 	RIFT_CMD_DISPLAY_INFO = 9,
-	RIFT_CMD_ENABLE_COMPONENTS = 0x1d
+	RIFT_CMD_ENABLE_COMPONENTS = 0x1d,
+	RIFT_CMD_POSITION_INFO = 15,
 } rift_sensor_feature_cmd;
 
 typedef enum {
@@ -96,12 +97,31 @@ typedef struct {
 	uint16_t keep_alive_interval;
 } pkt_keep_alive;
 
+typedef struct {
+	uint8_t flags;
+	int32_t pos_x;
+	int32_t pos_y;
+	int32_t pos_z;
+	int16_t dir_x;
+	int16_t dir_y;
+	int16_t dir_z;
+	uint8_t index;
+	uint8_t num;
+} pkt_position_info;
+
+typedef struct {
+	// Relative position in micrometers
+	vec3f pos;
+	// Normal
+	vec3f dir;
+} rift_led;
 
 bool decode_sensor_range(pkt_sensor_range* range, const unsigned char* buffer, int size);
 bool decode_sensor_display_info(pkt_sensor_display_info* info, const unsigned char* buffer, int size);
 bool decode_sensor_config(pkt_sensor_config* config, const unsigned char* buffer, int size);
 bool decode_tracker_sensor_msg(pkt_tracker_sensor* msg, const unsigned char* buffer, int size);
 bool decode_tracker_sensor_msg_dk2(pkt_tracker_sensor* msg, const unsigned char* buffer, int size);
+bool decode_position_info(pkt_position_info* p, const unsigned char* buffer, int size);
 
 void vec3f_from_rift_vec(const int32_t* smp, vec3f* out_vec);