/** @file vrpn_Oculus.C

@brief Drivers for various Oculus devices. Initially, just the DK1 & DK2.

@date 2015

@copyright 2015 ReliaSolve.com

@author ReliaSolve.com russ@reliasolve.com

@license Standard VRPN license.

*/

// Based on the OSVR hacker dev kit driver by Kevin Godby.

// Based on Oliver Kreylos' OculusRiftHIDReports.cpp and OculusRift.cpp.

// He has given permission to release this code under the VRPN license:

#include "vrpn_Oculus.h"

#include "vrpn_BaseClass.h" // for ::vrpn_TEXT_NORMAL, etc

VRPN_SUPPRESS_EMPTY_OBJECT_WARNING()

#if defined(VRPN_USE_HID)

// USB vendor and product IDs for the models we support

static const vrpn_uint16 OCULUS_VENDOR = 0x2833;

static const vrpn_uint16 DK1_PRODUCT = 0x0001;

static const vrpn_uint16 DK2_PRODUCT = 0x0021;

vrpn_Oculus::vrpn_Oculus(vrpn_uint16 product_id, vrpn_uint8 num_channels,

const char *name, vrpn_Connection *c, double keepAliveSeconds)

: vrpn_Analog(name, c)

, vrpn_HidInterface(m_filter =

new vrpn_HidProductAcceptor(OCULUS_VENDOR, product_id))

{

d_keepAliveSeconds = keepAliveSeconds;

vrpn_Analog::num_channel = num_channels;

memset(channel, 0, sizeof(channel));

memset(last, 0, sizeof(last));

// Set the timestamp

vrpn_gettimeofday(&d_timestamp, NULL);

d_lastKeepAlive = d_timestamp;

}

vrpn_Oculus::~vrpn_Oculus()

{

delete m_acceptor;

}

// Copied from Oliver Kreylos' OculusRift.cpp; he has given permission

// to release this code under the VRPN standard license.

// Unpacks a 3D vector from 8 bytes

inline void unpackVector(const vrpn_uint8 raw[8], int vector[3])

{

// @todo Make this also work on big-endian architectures.

union // Helper union to assemble 3 or 4 bytes into a signed 32-bit integer

{

vrpn_uint8 b[4];

vrpn_int32 i;

} p;

struct // Helper structure to sign-extend a 21-bit signed integer value

{

signed int si : 21;

} s;

/* Assemble the vector's x component: */

p.b[0] = raw[2];

p.b[1] = raw[1];

p.b[2] = raw[0];

// p.b[3]=0U; // Not needed because it's masked out below anyway

vector[0] = s.si = (p.i >> 3) & 0x001fffff;

/* Assemble the vector's y component: */

p.b[0] = raw[5];

p.b[1] = raw[4];

p.b[2] = raw[3];

p.b[3] = raw[2];

vector[1] = s.si = (p.i >> 6) & 0x001fffff;

/* Assemble the vector's z component: */

p.b[0] = raw[7];

p.b[1] = raw[6];

p.b[2] = raw[5];

// p.b[3]=0U; // Not needed because it's masked out below anyway

vector[2] = s.si = (p.i >> 1) & 0x001fffff;

}

// Thank you to Oliver Kreylos for the info needed to write this function.

// It is based on his OculusRiftHIDReports.cpp and OculusRift.cpp.

void vrpn_Oculus::parse_message_type_1(std::size_t bytes,

vrpn_uint8 *buffer)

{

size_t num_reports = buffer[1];

if (num_reports > 3) { num_reports = 3; }

// Skip past the report type and num_reports bytes and

// start parsing there.

vrpn_uint8 *bufptr = &buffer[2];

// The next two bytes are an increasing counter that changes by 1 for

// every report.

vrpn_uint16 report_index;

report_index = vrpn_unbuffer_from_little_endian<vrpn_uint16, vrpn_uint8>(bufptr);

channel[1] = report_index;

// The next two bytes are zero, so we skip them

vrpn_uint16 skip;

skip = vrpn_unbuffer_from_little_endian<vrpn_uint16, vrpn_uint8>(bufptr);

// The next entry is temperature, and it may be in hundredths of a degree C

vrpn_uint16 temperature;

const double temperature_scale = 0.01;

temperature = vrpn_unbuffer_from_little_endian<vrpn_uint16, vrpn_uint8>(bufptr);

channel[0] = temperature * temperature_scale;

// The magnetometer data comes after the space to store three

// reports.

vrpn_uint8 *magnetometer_ptr = &buffer[56];

vrpn_int16 magnetometer_raw[3];

for (size_t i = 0; i < 3; i++) {

magnetometer_raw[i] = vrpn_unbuffer_from_little_endian

<vrpn_int16, vrpn_uint8>(magnetometer_ptr);

}

// Invert all axes to make the magnetometer direction match

// the sign of the gravity vector.

const double magnetometer_scale = 0.0001;

channel[8] = -magnetometer_raw[0] * magnetometer_scale;

channel[9] = -magnetometer_raw[1] * magnetometer_scale;

channel[10] = -magnetometer_raw[2] * magnetometer_scale;

// Unpack a 16-byte accelerometer/gyro report using the routines from

// Oliver's code.

for (size_t i = 0; i < num_reports; i++) {

vrpn_int32 accelerometer_raw[3];

vrpn_int32 gyroscope_raw[3];

unpackVector(bufptr, accelerometer_raw);

bufptr += 8;

unpackVector(bufptr, gyroscope_raw);

bufptr += 8;

// Compute the double values using default calibration.

// The accelerometer data goes into analogs 0,1,2.

// The gyro data goes into analogs 3,4,5.

// The magnetomoter data goes into analogs 6,7,8.

const double accelerometer_scale = 0.0001;

const double gyroscope_scale = 0.0001;

channel[2] = accelerometer_raw[0] * accelerometer_scale;

channel[3] = accelerometer_raw[1] * accelerometer_scale;

channel[4] = accelerometer_raw[2] * accelerometer_scale;

channel[5] = gyroscope_raw[0] * gyroscope_scale;

channel[6] = gyroscope_raw[1] * gyroscope_scale;

channel[7] = gyroscope_raw[2] * gyroscope_scale;

vrpn_Analog::report_changes();

}

}

void vrpn_Oculus::mainloop()

{

vrpn_gettimeofday(&d_timestamp, NULL);

// See if it has been long enough to send another keep-alive

if (vrpn_TimevalDurationSeconds(d_timestamp, d_lastKeepAlive) >=

d_keepAliveSeconds) {

writeKeepAlive();

d_lastKeepAlive = d_timestamp;

}

update();

server_mainloop();

}

bool vrpn_Oculus::parse_message(std::size_t bytes,

vrpn_uint8 *buffer)

{

return false;

}

void vrpn_Oculus::on_data_received(std::size_t bytes,

vrpn_uint8 *buffer)

{

/* For debugging

printf("Got %d bytes:\n", static_cast<int>(bytes));

for (size_t i = 0; i < bytes; i++) {

printf("%02X ", buffer[i]);

}

printf("\n");

*/

// Set the timestamp for all reports

vrpn_gettimeofday(&d_timestamp, NULL);

// Make sure the message length and type is what we expect.

// We get 64-byte responses on Windows and 62-byte responses on the mac.

if ( (bytes != 62) && (bytes != 64) ) {

fprintf(stderr, "vrpn_Oculus::on_data_received(): Unexpected message length %d, ignoring\n",

static_cast<int>(bytes));

return;

}

switch(buffer[0]) {

case 1:

parse_message_type_1(bytes, buffer);

break;

default:

// Delegate message type to child

if (!parse_message(bytes, buffer)) {

fprintf(stderr, "vrpn_Oculus::on_data_received(): Unexpected message type %d, ignoring\n",

buffer[0]);

}

break;

}

}

vrpn_Oculus_DK1::vrpn_Oculus_DK1(const char *name,

vrpn_Connection *c, double keepAliveSeconds)

: vrpn_Oculus(DK1_PRODUCT, 11,

name, c, keepAliveSeconds) {};

// Thank you to Oliver Kreylos for the info needed to write this function.

// It is based on his OculusRiftHIDReports.cpp, used with permission.

void vrpn_Oculus_DK1::writeKeepAlive(

vrpn_uint16 interval

, vrpn_uint16 commandId)

{

// Buffer to store our report in.

vrpn_uint8 pktBuffer[5];

/* Pack the packet buffer, using little-endian packing: */

vrpn_uint8 *bufptr = pktBuffer;

vrpn_int32 buflen = sizeof(pktBuffer);

vrpn_buffer_to_little_endian(&bufptr, &buflen, vrpn_uint8(0x08U));

vrpn_buffer_to_little_endian(&bufptr, &buflen, commandId);

vrpn_buffer_to_little_endian(&bufptr, &buflen, interval);

/* Write the feature report: */

send_feature_report(sizeof(pktBuffer), pktBuffer);

}

vrpn_Oculus_DK2::vrpn_Oculus_DK2(bool enableLEDs,

const char *name, vrpn_Connection *c,

double keepAliveSeconds)

: vrpn_Oculus(DK2_PRODUCT, enableLEDs ? 12 : 11,

name, c, keepAliveSeconds)

{

d_enableLEDs = enableLEDs;

}

bool vrpn_Oculus_DK2::parse_message(std::size_t bytes,

vrpn_uint8 *buffer)

{

switch (buffer[0]) {

case 11:

parse_message_type_11(bytes, buffer);

break;

default:

return false;

}

return true;

}

// Thank you to Oliver Kreylos for the info needed to write this function.

// The actual order and meaning of fields was determined by walking through

// the packet to see what was in there, but the vector-decoding routines

// are used to pull out the inertial sensor data.

void vrpn_Oculus_DK2::parse_message_type_11(std::size_t bytes,

vrpn_uint8 *buffer)

{

// Started with the two different layouts in Oliver's code using my DK2

// and could not get the required data from it. I'm getting 64-byte

// reports that are somewhat like the 62-byte reports that code has but

// not the same. They seem closer to the code form the OculusRiftHIDReports.cpp

// document, but not the same (the last bytes are always 0, and they are supposed

// to be the magnetometer, for example).

// Looked at how the values changed and tried to figure out how to parse each

// part of the file.

// The first two bytes in the message are ignored; they are not present in the

// HID report for Oliver's code. The third byte is 0 and the fourth is the number

// of reports (up to 2, according to how much space is before the magnetometer

// data in the reports we found).

size_t num_reports = buffer[3];

if (num_reports > 2) { num_reports = 2; }

// @todo Check to see if we get multiple up reports

// when we get multiple other reports (I never see multiple

// other reports from my unit).

if (num_reports == 2) {

static bool printed = false;

if (!printed) {

fprintf(stderr, "vrpn_Oculus_DK2::on_data_received: Got 2 reports; check accuracy of "

"the up vector on the second and either replace with the first if it is zero or "

" remove this print statement from the code if it works.\n");

printed = true;

}

}

// Skip the first four bytes and start parsing the other reports from there.

vrpn_uint8 *bufptr = &buffer[4]; // Point at the start of the report

// The next two bytes seem to be an increasing counter that changes by 1 for

// every report.

vrpn_uint16 report_index, temperature;

report_index = vrpn_unbuffer_from_little_endian<vrpn_uint16, vrpn_uint8>(bufptr);

channel[1] = report_index;

// The next entry may be temperature, and it may be in hundredths of a degree C

const double temperature_scale = 0.01;

temperature = vrpn_unbuffer_from_little_endian<vrpn_uint16, vrpn_uint8>(bufptr);

channel[0] = temperature * temperature_scale;

// The next entry is a 4-byte counter with time since device was

// powered on in microseconds. We convert to seconds and report.

vrpn_uint32 microseconds;

microseconds = vrpn_unbuffer_from_little_endian<vrpn_uint32, vrpn_uint8>(bufptr);

channel[11] = microseconds * 1e-6;

// Unpack a 16-byte accelerometer/gyro report using the routines from

// Oliver's code. Also the magnetometer values(s).

// Then convert each to a scaled double value and send a report.

for (size_t i = 0; i < num_reports; i++) {

vrpn_int32 accelerometer_raw[3];

vrpn_int32 gyroscope_raw[3];

unpackVector(bufptr, accelerometer_raw);

bufptr += 8;

unpackVector(bufptr, gyroscope_raw);

bufptr += 8;

// Compute the double values using default calibration.

// The accelerometer data goes into analogs 0,1,2.

// The gyro data goes into analogs 3,4,5.

// The magnetomoter data goes into analogs 6,7,8.

const double accelerometer_scale = 0.0001;

const double gyroscope_scale = 0.0001;

channel[2] = accelerometer_raw[0] * accelerometer_scale;

channel[3] = accelerometer_raw[1] * accelerometer_scale;

channel[4] = accelerometer_raw[2] * accelerometer_scale;

channel[5] = gyroscope_raw[0] * gyroscope_scale;

channel[6] = gyroscope_raw[1] * gyroscope_scale;

channel[7] = gyroscope_raw[2] * gyroscope_scale;

// The magnetometer data comes after the space to store two

// reports. We don't know if there are two of them when

// there are two reports, but there is space in the report

// for it, so we try to decode two of them.

vrpn_uint8 *magnetometer_ptr = &buffer[44 + 8 * i];

vrpn_int16 magnetometer_raw[3];

for (size_t i = 0; i < 3; i++) {

magnetometer_raw[i] = vrpn_unbuffer_from_little_endian

<vrpn_int16, vrpn_uint8>(magnetometer_ptr);

}

// Invert these to make the magnetometer direction match

// the sign of the gravity vector.

const double magnetometer_scale = - 0.0001;

channel[8] = -magnetometer_raw[0] * magnetometer_scale;

channel[9] = -magnetometer_raw[1] * magnetometer_scale;

channel[10] = -magnetometer_raw[2] * magnetometer_scale;

vrpn_Analog::report_changes();

}

}

// Thank you to Oliver Kreylos for the info needed to write this function.

// It is based on his OculusRiftHIDReports.cpp, used with permission.

void vrpn_Oculus_DK2::writeKeepAlive(

vrpn_uint16 interval

, vrpn_uint16 commandId)

{

// Buffer to store our report in.

vrpn_uint8 pktBuffer[6];

/* Pack the packet buffer, using little-endian packing: */

vrpn_uint8 *bufptr = pktBuffer;

vrpn_int32 buflen = sizeof(pktBuffer);

vrpn_buffer_to_little_endian(&bufptr, &buflen, vrpn_uint8(0x11U));

vrpn_buffer_to_little_endian(&bufptr, &buflen, commandId);

vrpn_uint8 flags = d_enableLEDs ? 0x0bU : 0x01U;

vrpn_buffer_to_little_endian(&bufptr, &buflen, flags);

vrpn_buffer_to_little_endian(&bufptr, &buflen, interval);

/* Write the LED control feature report: */

send_feature_report(sizeof(pktBuffer), pktBuffer);

}

#endif