Merge pull request #107 from OpenHMD/dk2-led

Added LED management for Oculus DK2 and CV1

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,34 @@
 #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;
+	SKIP8;
+	p->type = READ8;
+
+	return true;
+}
+
 bool decode_sensor_range(pkt_sensor_range* range, const unsigned char* buffer, int size)
 {
 	if(!(size == 8 || size == 9)){
@@ -195,7 +224,7 @@ int encode_keep_alive(unsigned char* buffer, const pkt_keep_alive* keep_alive)
 	return 5; // keep alive packet size
 }
 
-int encode_enable_components(unsigned char* buffer, bool display, bool audio)
+int encode_enable_components(unsigned char* buffer, bool display, bool audio, bool leds)
 {
 	uint8_t flags = 0;
 
@@ -203,12 +232,13 @@ int encode_enable_components(unsigned char* buffer, bool display, bool audio)
 	WRITE16(0); // last command ID
 
 	if (display)
-		flags |= 1;
+		flags |= RIFT_COMPONENT_DISPLAY;
 	if (audio)
-		flags |= 2;
-//	flags |= 4; // I don't know what it is. Wireless?
+		flags |= RIFT_COMPONENT_AUDIO;
+	if (leds)
+		flags |= RIFT_COMPONENT_LEDS;
 	WRITE8(flags);
-	return 4;
+	return 4; // component flags packet size
 }
 
 void dump_packet_sensor_range(const pkt_sensor_range* range)

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 {
@@ -286,9 +292,47 @@ static ohmd_device* open_device(ohmd_driver* driver, ohmd_device_desc* desc)
 	// Turn the screens on
 	if (desc->revision == REV_CV1)
 	{
-		size = encode_enable_components(buf, true, true);
+		size = encode_enable_components(buf, true, true, true);
 		if (send_feature_report(priv, buf, size) == -1)
 			LOGE("error turning the screens on");
+
+		hid_write(priv->handle, rift_enable_leds_cv1, sizeof(rift_enable_leds_cv1));
+	}
+	else if (desc->revision == REV_DK2)
+	{
+		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

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 {
@@ -38,6 +39,12 @@ typedef enum {
 	RIFT_DT_DISTORTION
 } rift_distortion_type;
 
+typedef enum {
+	RIFT_COMPONENT_DISPLAY = 1,
+	RIFT_COMPONENT_AUDIO = 2,
+	RIFT_COMPONENT_LEDS = 4
+} rift_component_type;
+
 // Sensor config flags
 #define RIFT_SCF_RAW_MODE           0x01
 #define RIFT_SCF_CALIBRATION_TEST   0x02
@@ -47,7 +54,13 @@ typedef enum {
 #define RIFT_SCF_COMMAND_KEEP_ALIVE 0x20
 #define RIFT_SCF_SENSOR_COORDINATES 0x40
 
+static const unsigned char rift_enable_leds_dk2[17] = {
+	0x0c, 0x00, 0x00, 0x00, 0x01, 0x00, 0x5E, 0x01, 0x1A, 0x41, 0x00, 0x00, 0x7F,
+};
 
+static const unsigned char rift_enable_leds_cv1[17] = {
+	0x0c, 0x00, 0x00, 0xFF, 0x05, 0x00, 0x8F, 0x01, 0x00, 0x4B, 0x00, 0x00, 0x7F,
+};
 
 typedef struct {
 	uint16_t command_id;
@@ -94,18 +107,38 @@ 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;
+	uint8_t type;
+} 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);
 
 int encode_sensor_config(unsigned char* buffer, const pkt_sensor_config* config);
 int encode_keep_alive(unsigned char* buffer, const pkt_keep_alive* keep_alive);
-int encode_enable_components(unsigned char* buffer, bool display, bool audio);
+int encode_enable_components(unsigned char* buffer, bool display, bool audio, bool leds);
 
 void dump_packet_sensor_range(const pkt_sensor_range* range);
 void dump_packet_sensor_config(const pkt_sensor_config* config);