aboutsummaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
-rw-r--r--Bindings/Java/pom.xml19
-rw-r--r--Bindings/Java/src/main/java/org/saintandreas/input/hid/HidFeatureReport.java77
-rw-r--r--Bindings/Java/src/main/java/org/saintandreas/oculus/RiftTracker.java148
-rwxr-xr-xBindings/Java/src/main/java/org/saintandreas/oculus/fusion/SensorFilter.java56
-rwxr-xr-xBindings/Java/src/main/java/org/saintandreas/oculus/fusion/SensorFusion.java895
-rw-r--r--Bindings/Java/src/main/java/org/saintandreas/oculus/hid/DisplayInfo.java75
-rw-r--r--Bindings/Java/src/main/java/org/saintandreas/oculus/hid/KeepAlive.java41
-rw-r--r--Bindings/Java/src/main/java/org/saintandreas/oculus/hid/SensorConfig.java59
-rw-r--r--Bindings/Java/src/main/java/org/saintandreas/oculus/hid/SensorRange.java52
-rw-r--r--Bindings/Java/src/main/java/org/saintandreas/oculus/tracker/TrackerMessage.java32
-rw-r--r--Bindings/Java/src/main/java/org/saintandreas/oculus/tracker/TrackerSample.java13
-rw-r--r--Bindings/Java/src/main/java/org/saintandreas/oculus/tracker/TrackerVector.java49
-rw-r--r--Bindings/Java/src/test/java/org/saintandreas/input/oculus/RiftTest.java81
13 files changed, 1588 insertions, 9 deletions
diff --git a/Bindings/Java/pom.xml b/Bindings/Java/pom.xml
index 958f848..3b264b0 100644
--- a/Bindings/Java/pom.xml
+++ b/Bindings/Java/pom.xml
@@ -1,13 +1,13 @@
<project xmlns="http://maven.apache.org/POM/4.0.0" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
xsi:schemaLocation="http://maven.apache.org/POM/4.0.0 http://maven.apache.org/xsd/maven-4.0.0.xsd">
<modelVersion>4.0.0</modelVersion>
- <groupId>com.oculusvr</groupId>
+ <groupId>org.saintandreas</groupId>
<artifactId>jocular</artifactId>
- <version>1.0.0-SNAPSHOT</version>
+ <version>0.0.1-SNAPSHOT</version>
<scm>
- <connection>scm:git:git://[email protected]/jherico/OculusSDK.git</connection>
- <developerConnection>scm:git:git://[email protected]/jherico/OculusSDK.git</developerConnection>
+ <connection>scm:git:https://github.com/jherico/OculusSDK.git</connection>
+ <developerConnection>scm:git:[email protected]:jherico/OculusSDK.git</developerConnection>
<url>https://github.com/jherico/OculusSDK</url>
</scm>
@@ -46,6 +46,7 @@
<plugins>
<plugin>
<artifactId>maven-compiler-plugin</artifactId>
+ <version>2.3.2</version>
<configuration>
<source>1.7</source>
<target>1.7</target>
@@ -63,16 +64,16 @@
<version>[1, )</version>
</dependency>
<dependency>
+ <groupId>com.codeminders</groupId>
+ <artifactId>hidapi</artifactId>
+ <version>[1.1, 2)</version>
+ </dependency>
+ <dependency>
<groupId>com.google.guava</groupId>
<artifactId>guava</artifactId>
<version>[14, )</version>
</dependency>
<dependency>
- <groupId>com.codeminders</groupId>
- <artifactId>hidapi</artifactId>
- <version>[1.1, )</version>
- </dependency>
- <dependency>
<groupId>org.slf4j</groupId>
<artifactId>slf4j-api</artifactId>
<version>${slf4j.version}</version>
diff --git a/Bindings/Java/src/main/java/org/saintandreas/input/hid/HidFeatureReport.java b/Bindings/Java/src/main/java/org/saintandreas/input/hid/HidFeatureReport.java
new file mode 100644
index 0000000..3e7da6a
--- /dev/null
+++ b/Bindings/Java/src/main/java/org/saintandreas/input/hid/HidFeatureReport.java
@@ -0,0 +1,77 @@
+/*
+ * Copyright (c) 2013 Bradley Austin Davis <[email protected]>. All rights
+ * reserved.
+ *
+ * Redistribution and use in source and binary forms are permitted provided that
+ * the above copyright notice and this paragraph are duplicated in all such
+ * forms and that any documentation, advertising materials, and other materials
+ * related to such distribution and use acknowledge that the software was
+ * developed by the <organization>. The name of the <organization> may not be
+ * used to endorse or promote products derived from this software without
+ * specific prior written permission. THIS SOFTWARE IS PROVIDED ``AS IS'' AND
+ * WITHOUT ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, WITHOUT LIMITATION, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ */
+
+package org.saintandreas.input.hid;
+
+import java.io.IOException;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+
+import com.codeminders.hidapi.HIDDevice;
+
+/**
+ * Encapsulate a feature report from HID. Derived classes must know their size
+ * and feature ID as well as how to serialize to and from a bytebuffer
+ *
+ */
+public abstract class HidFeatureReport {
+ public final byte featureId;
+ public final int size;
+
+ public HidFeatureReport(byte featureId, int size) {
+ this.featureId = featureId;
+ this.size = size;
+ }
+
+ public HidFeatureReport(byte featureId, int size, HIDDevice device) throws IOException {
+ this.featureId = featureId;
+ this.size = size;
+ read(device);
+ }
+
+ public final byte getFeatureId() {
+ return featureId;
+ }
+
+ public final int getSize() {
+ return size;
+ }
+
+ private ByteBuffer allocate() {
+ ByteBuffer result = ByteBuffer.allocate(getSize()).order(ByteOrder.LITTLE_ENDIAN);
+ result.put(getFeatureId());
+ return result;
+ }
+
+ protected abstract void parse(ByteBuffer buffer);
+
+ protected abstract void pack(ByteBuffer buffer);
+
+ public void read(HIDDevice device) throws IOException {
+ ByteBuffer buffer = allocate();
+ device.getFeatureReport(buffer.array());
+ buffer.position(0);
+ if (this.featureId != buffer.get()) {
+ throw new IllegalStateException();
+ }
+ parse(buffer);
+ }
+
+ public void write(HIDDevice device) throws IOException {
+ ByteBuffer buffer = allocate();
+ pack(buffer);
+ device.sendFeatureReport(buffer.array());
+ }
+} \ No newline at end of file
diff --git a/Bindings/Java/src/main/java/org/saintandreas/oculus/RiftTracker.java b/Bindings/Java/src/main/java/org/saintandreas/oculus/RiftTracker.java
new file mode 100644
index 0000000..0588fcd
--- /dev/null
+++ b/Bindings/Java/src/main/java/org/saintandreas/oculus/RiftTracker.java
@@ -0,0 +1,148 @@
+package org.saintandreas.oculus;
+
+import java.io.IOException;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+
+import org.saintandreas.oculus.hid.DisplayInfo;
+import org.saintandreas.oculus.hid.KeepAlive;
+import org.saintandreas.oculus.hid.SensorConfig;
+import org.saintandreas.oculus.hid.SensorRange;
+import org.saintandreas.oculus.tracker.TrackerMessage;
+
+import com.codeminders.hidapi.HIDDevice;
+import com.codeminders.hidapi.HIDDeviceInfo;
+import com.codeminders.hidapi.HIDManager;
+import com.google.common.base.Predicate;
+
+public final class RiftTracker implements Runnable {
+
+ public static final int SENSOR_VENDOR_ID = 0x2833;
+ public static final int SENSOR_PRODUCT_ID = 0x0001;
+ private static final short KEEP_ALIVE_TIME = 10 * 1000;
+ private static final short KEEP_ALIVE_INTERVAL = 3 * 1000;
+
+ // The device provided by the hidapi library
+ private final HIDDevice device;
+ private final Thread listener;
+ private final ByteBuffer buffer = ByteBuffer.allocate(62).order(ByteOrder.nativeOrder());
+ private volatile boolean shutdown = false;
+ private Predicate<TrackerMessage> callback = null;
+
+ private RiftTracker(HIDDevice device) {
+ this.device = device;
+ this.listener = new Thread(this);
+ }
+
+ public void start(Predicate<TrackerMessage> callback) {
+ this.callback = callback;
+ this.listener.start();
+ }
+
+ public synchronized void stop() {
+ if (listener.isAlive()) {
+ shutdown = true;
+ try {
+ listener.join();
+ } catch (InterruptedException e) {
+ throw new IllegalStateException(e);
+ }
+ try {
+ device.close();
+ } catch (IOException e1) {
+ }
+ }
+ }
+
+ @Override
+ public void run() {
+ long nextKeepAlive = 0;
+ int bytesRead = 0;
+ try {
+ device.enableBlocking();
+ while (!shutdown) {
+ if (System.currentTimeMillis() > nextKeepAlive) {
+ setKeepAliveMilliSeconds(KEEP_ALIVE_TIME);
+ nextKeepAlive = System.currentTimeMillis() + KEEP_ALIVE_INTERVAL;
+ }
+ int timeout = (int) Math.max(System.currentTimeMillis() - nextKeepAlive, 100);
+ bytesRead = device.readTimeout(buffer.array(), timeout);
+ if (0 == bytesRead) {
+ continue;
+ }
+ buffer.position(0);
+ callback.apply(new TrackerMessage(buffer));
+ }
+ } catch (IOException e) {
+ throw new IllegalStateException(e);
+ }
+ }
+
+ public DisplayInfo getDisplayInfo() throws IOException {
+ DisplayInfo result = new DisplayInfo(device);
+ return result;
+ }
+
+ public SensorRange getSensorRange() throws IOException {
+ return new SensorRange(device);
+ }
+
+ public SensorConfig getSensorConfig() throws IOException {
+ return new SensorConfig(device);
+ }
+
+ public void setSensorConfig(SensorConfig sc) throws IOException {
+ sc.write(device);
+ }
+
+ public KeepAlive getKeepAlive() throws IOException {
+ return new KeepAlive(device);
+ }
+
+ public void setKeepAlive(KeepAlive ka) throws IOException {
+ ka.write(device);
+ }
+
+ public void setKeepAliveMilliSeconds(short milliseconds) throws IOException {
+ this.setKeepAlive(new KeepAlive(milliseconds));
+ }
+
+ private static HIDDevice loadDevice() throws IOException {
+ com.codeminders.hidapi.ClassPathLibraryLoader.loadNativeHIDLibrary();
+ HIDManager mgr = HIDManager.getInstance();
+
+ HIDDeviceInfo[] devs = mgr.listDevices();
+ for (HIDDeviceInfo d : devs)
+ {
+ if (d.getVendor_id() == SENSOR_VENDOR_ID && d.getProduct_id() == SENSOR_PRODUCT_ID)
+ return d.open();
+ }
+
+ return mgr.openById(SENSOR_VENDOR_ID, SENSOR_PRODUCT_ID, null);
+ }
+
+ private static RiftTracker INSTANCE = null;
+
+ public synchronized static RiftTracker getInstance() throws IOException {
+ if (null == INSTANCE) {
+ INSTANCE = new RiftTracker(loadDevice());
+ }
+ return INSTANCE;
+ }
+
+ public static synchronized void startListening(Predicate<TrackerMessage> callback) throws IOException {
+ getInstance().start(callback);
+ }
+
+ public static synchronized void stopListening() {
+ if (null == INSTANCE) {
+ return;
+ }
+
+ try {
+ INSTANCE.stop();
+ } finally {
+ INSTANCE = null;
+ }
+ }
+}
diff --git a/Bindings/Java/src/main/java/org/saintandreas/oculus/fusion/SensorFilter.java b/Bindings/Java/src/main/java/org/saintandreas/oculus/fusion/SensorFilter.java
new file mode 100755
index 0000000..f2bb747
--- /dev/null
+++ b/Bindings/Java/src/main/java/org/saintandreas/oculus/fusion/SensorFilter.java
@@ -0,0 +1,56 @@
+package org.saintandreas.oculus.fusion;
+
+import org.saintandreas.math.Vector3f;
+
+public class SensorFilter {
+ private final Vector3f[] queue;
+ private int head = 0;
+ private int size = 0;
+
+ SensorFilter(int i) {
+ queue = new Vector3f[i];
+ };
+
+ // Create a new element to the filter
+ public void AddElement(Vector3f e) {
+ queue[head++] = e;
+ head %= queue.length;
+ if (size < queue.length) {
+ ++size;
+ }
+ };
+
+ public Vector3f Total() {
+ Vector3f total = Vector3f.ZERO;
+ for (Vector3f v : queue) {
+ if (v == null) {
+ break;
+ }
+ total = total.add(v);
+ }
+ return total;
+ }
+
+ public Vector3f Mean() {
+ return Total().mult(1 / (float) size);
+ }
+
+ public Vector3f GetPrev(int i) {
+ int index = head - (i + 1);
+ if (index < 0) {
+ index += queue.length;
+ }
+ return queue[i];
+ }
+
+ Vector3f SavitzkyGolaySmooth8() {
+ return GetPrev(0).mult(0.41667f).add(
+ GetPrev(1).mult(0.33333f)).add(
+ GetPrev(2).mult(0.25f)).add(
+ GetPrev(3).mult(0.1667f)).add(
+ GetPrev(4).mult(0.08333f)).subtract(
+ GetPrev(6).mult(0.08333f)).subtract(
+ GetPrev(7).mult(0.1667f));
+ }
+
+}
diff --git a/Bindings/Java/src/main/java/org/saintandreas/oculus/fusion/SensorFusion.java b/Bindings/Java/src/main/java/org/saintandreas/oculus/fusion/SensorFusion.java
new file mode 100755
index 0000000..23bdfe3
--- /dev/null
+++ b/Bindings/Java/src/main/java/org/saintandreas/oculus/fusion/SensorFusion.java
@@ -0,0 +1,895 @@
+package org.saintandreas.oculus.fusion;
+
+import org.saintandreas.math.Matrix4f;
+import org.saintandreas.math.Quaternion;
+import org.saintandreas.math.Vector3f;
+import org.saintandreas.oculus.tracker.TrackerMessage;
+import org.saintandreas.oculus.tracker.TrackerSample;
+import org.saintandreas.oculus.tracker.TrackerVector;
+
+import com.google.common.base.Predicate;
+
+// -------------------------------------------------------------------------------------
+// ***** SensorFusion
+
+// SensorFusion class accumulates Sensor notification messages to keep track of
+// orientation, which involves integrating the gyro and doing correction with
+// gravity.
+//
+// Magnetometer based yaw drift correction is also supported; it is usually
+// enabled automatically based on loaded magnetometer configuration.
+//
+// Orientation is reported as a quaternion, from which users can obtain either
+// the rotation matrix or Euler angles.
+//
+// The class can operate in two ways:
+// - By user manually passing MessageBodyFrame messages to the OnMessage()
+// function.
+// - By attaching SensorFusion to a SensorDevice, in which case it will
+// automatically handle notifications from that device.
+
+@SuppressWarnings("unused")
+public class SensorFusion {
+ private boolean yawCorrectionEnabled = false;
+ private boolean motionTrackingEnabled = true;
+ private boolean gravityEnabled = true;
+ private boolean predictionEnabled = true;
+
+ private Quaternion Q = new Quaternion();
+ private Quaternion QUncorrected = new Quaternion();
+
+ private Vector3f RawAccel = new Vector3f();
+ private Vector3f AngV = new Vector3f();
+ private float AngVL = 0;
+ private Vector3f CalMag = new Vector3f();
+ private Vector3f RawMag = new Vector3f();
+
+ private int Stage = 0;
+ private float RunningTime = 0;
+ private float DeltaT = 0.001f;
+ private float Gain = 0.05f;
+ private float YawMult = 1.0f;
+ private float PredictionDT = 0.03f;
+
+ private final SensorFilter FRawMag = new SensorFilter(10);
+ private final SensorFilter FAccW = new SensorFilter(20);
+ private final SensorFilter FAngV = new SensorFilter(20);
+
+ private int TiltCondCount = 0;
+ private float TiltErrorAngle = 0;
+ private Vector3f TiltErrorAxis = Vector3f.UNIT_Y;
+
+ // Obtain the current accumulated orientation. Many apps will want to use
+ // GetPredictedOrientation instead to reduce latency.
+ public synchronized Quaternion getOrientation() {
+ return Q;
+ }
+
+ public Quaternion getPredictedOrientation() {
+ return getPredictedOrientation(PredictionDT);
+ }
+
+ // Obtain the last absolute acceleration reading, in m/s^2.
+ public synchronized Vector3f getAcceleration() {
+ return RawAccel;
+ }
+
+ // Obtain the last angular velocity reading, in rad/s.
+ public synchronized Vector3f getAngularVelocity() {
+ return AngV;
+ }
+
+ // Obtain the last raw magnetometer reading, in Gauss
+ public synchronized Vector3f getMagnetometer() {
+ return RawMag;
+ }
+
+ // Obtain the calibrated magnetometer reading (direction and field strength)
+ synchronized Vector3f getCalibratedMagnetometer() {
+ return CalMag;
+ }
+
+ // Resets the current orientation.
+ public void setMotionTrackingEnabled(boolean enable) {
+ motionTrackingEnabled = enable;
+ }
+
+ public boolean isMotionTrackingEnabled() {
+ return motionTrackingEnabled;
+ }
+
+ // Multiplier for yaw rotation (turning); setting this higher than 1 (the
+ // default) can allow the game
+ // to be played without auxillary rotation controls, possibly making it more
+ // immersive.
+ // Whether this is more or less likely to cause motion sickness is unknown.
+ public float GetYawMultiplier() {
+ return YawMult;
+ }
+
+ public void SetYawMultiplier(float y) {
+ YawMult = y;
+ }
+
+ // *** Prediction Control
+
+ // Prediction delta specifes how much prediction should be applied in
+ // seconds; it should in general be under the average rendering latency.
+ // Call GetPredictedOrientation() to get predicted orientation.
+ public float GetPredictionDelta() {
+ return PredictionDT;
+ }
+
+ public void SetPrediction(float dt) {
+ PredictionDT = dt;
+ predictionEnabled = true;
+ }
+
+ public void SetPredictionEnabled(boolean enable) {
+ predictionEnabled = enable;
+ }
+
+ public boolean IsPredictionEnabled() {
+ return predictionEnabled;
+ }
+
+ // *** Accelerometer/Gravity Correction Control
+
+ // Enables/disables gravity correction (on by default).
+ public void SetGravityEnabled(boolean enable) {
+ gravityEnabled = enable;
+ }
+
+ public boolean IsGravityEnabled() {
+ return gravityEnabled;
+ }
+
+ // Gain used to correct gyro with accel. Default value is appropriate for
+ // typical use.
+ public float GetAccelGain() {
+ return Gain;
+ }
+
+ public void SetAccelGain(float ag) {
+ Gain = ag;
+ }
+
+ // *** Magnetometer and Yaw Drift Correction Control
+
+ // Methods to load and save a mag calibration. Calibrations can optionally
+ // be specified by name to differentiate multiple calibrations under
+ // different conditions
+ // If LoadMagCalibration succeeds, it will override YawCorrectionEnabled
+ // based on
+ // saved calibration setting.
+ public boolean SaveMagCalibration(String file) {
+ return false;
+ }
+
+ public boolean LoadMagCalibration(String file) {
+ return false;
+ }
+
+ // Enables/disables magnetometer based yaw drift correction. Must also have
+ // mag calibration data for this correction to work. void
+ public void SetYawCorrectionEnabled(boolean enable) {
+ yawCorrectionEnabled = enable;
+ }
+
+ // Determines if yaw correction is enabled.
+ public boolean IsYawCorrectionEnabled() {
+ return yawCorrectionEnabled;
+ }
+
+ // -------------------------------------------------------------------------------------
+ // ***** Sensor Fusion
+
+ // Resets the current orientation
+ public synchronized void Reset() {
+ QUncorrected = Q = Quaternion.IDENTITY;
+ Stage = 0;
+ RunningTime = 0;
+ // MagNumReferences = 0;
+ // MagHasNearbyReference = false;
+ }
+
+ // Sensor reports data in the following coordinate system:
+ // Accelerometer: 10^-4 m/s^2; X forward, Y right, Z Down.
+ // Gyro: 10^-4 rad/s; X positive roll right, Y positive pitch up; Z positive
+ // yaw right.
+ void AccelFromBodyFrameUpdate(TrackerVector in) {
+ RawAccel = new Vector3f(in.x, in.y, in.z).mult(0.0001f);
+ }
+
+ void MagFromBodyFrameUpdate(TrackerVector in) {
+ RawMag = new Vector3f(in.x, in.y, in.z).mult(0.0001f);
+ }
+
+ void EulerFromBodyFrameUpdate(TrackerVector in) {
+ AngV = new Vector3f(in.x, in.y, in.z).mult(0.0001f);
+ AngVL = AngV.length();
+ }
+
+ class FusionHandler implements Predicate<TrackerMessage> {
+ private long LastTimestamp = -1;
+ private int LastSampleCount = -1;
+
+ // private final Vector3f LastMag = new Vector3f();
+ // private final Vector3f LastGyro = new Vector3f();
+ // private final Vector3f LastAccel = new Vector3f();
+
+ @Override
+ public boolean apply(TrackerMessage t) {
+ synchronized (SensorFusion.this) {
+ if (LastTimestamp != -1) {
+ long timestampDelta = t.timestamp - LastTimestamp;
+ if (t.timestamp < LastTimestamp) {
+ timestampDelta += 0x10000;
+ }
+
+ // If we missed a small number of samples, replicate the
+ // last sample.
+ if ((timestampDelta > LastSampleCount) && (timestampDelta <= 254)) {
+ DeltaT = (timestampDelta - t.sampleCount) * 0.001f;
+ processSensorMessage();
+ DeltaT = 0.001f;
+ }
+ }
+
+ LastSampleCount = t.sampleCount;
+ LastTimestamp = t.timestamp;
+ MagFromBodyFrameUpdate(t.mag);
+ // We've exceeded the max number of reportable samples, so
+ for (int i = 0; i < Math.min(t.sampleCount, 3); ++i) {
+ TrackerSample ts = t.samples.get(0);
+ EulerFromBodyFrameUpdate(ts.gyro);
+ AccelFromBodyFrameUpdate(ts.accel);
+ processSensorMessage();
+ }
+ }
+ return true;
+ }
+ }
+
+ public FusionHandler createHandler() {
+ return new FusionHandler();
+ }
+
+ private static final float GRAVITY_EPSILON = 0.4f;
+ // Relatively slow rotation
+ private static final float GYRO_EPSILON = 0.1f;
+ // Required time steps of stability
+ private static final int TILT_PERIOD = 50;
+ private static final float MAX_TILT_ERROR = 0.05f;
+ private static final float MIN_TILT_ERROR = 0.01f;
+
+ public void processSensorMessage() {
+ if (!isMotionTrackingEnabled())
+ return;
+
+ // Apply the calibration parameters to raw mag
+ // if (HasMagCalibration())
+ // {
+ // mag.x += MagCalibrationMatrix.M[0][3];
+ // mag.y += MagCalibrationMatrix.M[1][3];
+ // mag.z += MagCalibrationMatrix.M[2][3];
+ // }
+ // // Provide external access to calibrated mag values
+ // // (if the mag is not calibrated, then the raw value is returned)
+ // CalMag = mag;
+
+ // Keep track of time
+ Stage++;
+ RunningTime += DeltaT;
+
+ // Insert current sensor data into filter history
+ FRawMag.AddElement(RawMag);
+ // Acceleration in the world frame (Q is current HMD orientation)
+ FAccW.AddElement(Q.mult(RawAccel));
+ FAngV.AddElement(AngV);
+
+ // Update orientation Q based on gyro outputs. This technique is
+ // based on direct properties of the angular velocity vector:
+ // Its direction is the current rotation axis, and its magnitude
+ // is the rotation rate (rad/sec) about that axis. Our sensor
+ // sampling rate is so fast that we need not worry about integral
+ // approximation error (not yet, anyway).
+ if (AngVL > 0.0f) {
+ Q = Q.mult(Quaternion.fromAngleAxis(AngVL * DeltaT, AngV.divide(AngVL)));
+ }
+
+ // The quaternion magnitude may slowly drift due to numerical error,
+ // so it is periodically normalized.
+ if (Stage % 5000 == 0) {
+ Q = Q.normalize();
+ }
+
+ // Maintain the uncorrected orientation for later use by predictive
+ // filtering
+ QUncorrected = Q;
+
+ // Perform tilt correction using the accelerometer data. This enables
+ // drift errors in pitch and roll to be corrected. Note that yaw cannot
+ // be corrected because the rotation axis is parallel to the gravity
+ // vector.
+ if (gravityEnabled) {
+ // Correcting for tilt error by using accelerometer data
+ tiltCorrect();
+ } // Tilt correction
+
+ // if (yawCorrectionEnabled) {
+ // yawCorrect();
+ // }
+
+ // Warning: If YawMult != 1, then AngV is not true angular velocity
+ AngV = new Vector3f(AngV.x, AngV.y * YawMult, AngV.z);
+ }
+
+ private static final float MIN_PDT = 0.001f;
+ private static final float SLOPE_PDT = 0.1f;
+
+ private void tiltCorrect() {
+ // This condition estimates whether the only measured acceleration
+ // is due to gravity
+ // (the Rift is not linearly accelerating). It is often wrong, but
+ // tends to average
+ // out well over time.
+ float accLength = RawAccel.length();
+ if ((Math.abs(accLength - 9.81f) < GRAVITY_EPSILON)
+ && (AngVL < GYRO_EPSILON))
+ TiltCondCount++;
+ else
+ TiltCondCount = 0;
+
+ // After stable measurements have been taken over a sufficiently
+ // long period, estimate the amount of tilt error and calculate the
+ // tilt axis for later correction.
+ if (TiltCondCount >= TILT_PERIOD) {
+ // Update TiltErrorEstimate
+ TiltCondCount = 0;
+ // Use an average value to reduce noise (could alternatively use
+ // an LPF)
+ Vector3f accWMean = FAccW.Mean();
+ // This is the amount of rotation
+ float tiltAngle = Vector3f.UNIT_Y.angleBetween(accWMean.normalize());
+ // Record values if the tilt error is intolerable
+ if (tiltAngle > MAX_TILT_ERROR) {
+ // Project the acceleration vector into the XZ plane
+ // The unit normal of xzAcc will be the rotation axis for
+ // tilt correction
+ TiltErrorAngle = tiltAngle;
+ TiltErrorAxis = new Vector3f(accWMean.z, 0.0f, -accWMean.x)
+ .normalize();
+ }
+ }
+
+ // This part performs the actual tilt correction as needed
+ if (TiltErrorAngle > MIN_TILT_ERROR) {
+ if ((TiltErrorAngle > 0.4f) && (RunningTime < 8.0f)) {
+ // Tilt completely to correct orientation
+ Q = Q.mult(Quaternion.fromAngleAxis(-TiltErrorAngle, TiltErrorAxis));
+ TiltErrorAngle = 0.0f;
+ } else {
+ // This uses aggressive correction steps while your head is
+ // moving fast
+ float deltaTiltAngle = -Gain * TiltErrorAngle * 0.005f
+ * (5.0f * AngVL + 1.0f);
+ // Incrementally "un-tilt" by a small step size
+ Q = Q.mult(Quaternion.fromAngleAxis(deltaTiltAngle, TiltErrorAxis));
+ TiltErrorAngle += deltaTiltAngle;
+ }
+ }
+ }
+
+ // A predictive filter based on extrapolating the smoothed, current angular
+ // velocity
+ public synchronized Quaternion getPredictedOrientation(float pdt) {
+ Quaternion qP = QUncorrected;
+ if (predictionEnabled) {
+ // This method assumes a constant angular velocity
+ // Vector3f angVelF = FAngV.SavitzkyGolaySmooth8();
+ // Force back to raw measurement
+ Vector3f angVelF = AngV;
+ float angVelFL = angVelF.length();
+
+ // Dynamic prediction interval: Based on angular velocity to reduce
+ // vibration
+ float newpdt = pdt;
+ float tpdt = MIN_PDT + SLOPE_PDT * angVelFL;
+ if (tpdt < pdt) {
+ newpdt = tpdt;
+ }
+
+ if (angVelFL > 0.001f) {
+ qP = qP.mult(Quaternion.fromAngleAxis(angVelFL * newpdt,
+ angVelF.divide(angVelFL)));
+ }
+ }
+ return qP;
+ }
+
+ private static final float maxAngVelLength = 3.0f;
+ private static final int magWindow = 5;
+ private static final float yawErrorMax = 0.1f;
+ private static final float yawErrorMin = 0.01f;
+ private static final int yawErrorCountLimit = 50;
+ private static final float yawRotationStep = 0.00002f;
+ private static final int MagMaxReferences = 80;
+
+ public class MagCalibration {
+ Matrix4f MagCalibrationMatrix = new Matrix4f();
+ long MagCalibrationTime = -1;;
+ boolean MagCalibrated = false;
+ int MagCondCount = 0;
+ float MagRefDistance = 0.5f;
+ Quaternion MagRefQ = new Quaternion();
+ Vector3f MagRefM = Vector3f.ZERO;
+ float MagRefYaw = 0;
+ boolean MagHasNearbyReference = false;
+ final Quaternion[] MagRefTableQ = new Quaternion[MagMaxReferences];
+ final Vector3f[] MagRefTableM = new Vector3f[MagMaxReferences];
+ final float[] MagRefTableYaw = new float[MagMaxReferences];
+ int MagNumReferences = 0;
+ float YawErrorAngle = 0;
+ int YawErrorCount = 0;
+ boolean YawCorrectionInProgress = false;
+ boolean YawCorrectionActivated = false;
+
+ Vector3f getCalibratedMagValue(Vector3f rawMag) {
+ Vector3f mag = rawMag;
+ // mag.x += MagCalibrationMatrix.M[0][3];
+ // mag.y += MagCalibrationMatrix.M[1][3];
+ // mag.z += MagCalibrationMatrix.M[2][3];
+ return mag;
+ }
+
+ void setMagReference(Quaternion q, Vector3f rawMag) {
+ if (MagNumReferences < MagMaxReferences) {
+ MagRefTableQ[MagNumReferences] = q;
+ MagRefTableM[MagNumReferences] = rawMag; // FRawMag.Mean();
+ MagRefQ = q;
+ MagRefM = rawMag;
+ float yaw = q.toAngles(null)[0];
+ MagRefTableYaw[MagNumReferences] = yaw;
+ MagRefYaw = yaw;
+ MagNumReferences++;
+
+ MagHasNearbyReference = true;
+ }
+ }
+
+ // Yaw correction is currently working (forcing a corrective yaw *
+ // rotation)
+ boolean IsYawCorrectionInProgress() {
+ return YawCorrectionInProgress;
+ }
+
+ // Store the calibration matrix for the magnetometer
+ void SetMagCalibration(Matrix4f m) {
+ MagCalibrationMatrix = m;
+ MagCalibrationTime = System.currentTimeMillis();
+ MagCalibrated = true;
+ }
+
+ // Retrieves the magnetometer calibration matrix
+ Matrix4f GetMagCalibration() {
+ return MagCalibrationMatrix;
+ }
+
+ // Retrieve the time of the calibration
+ long GetMagCalibrationTime() {
+ return MagCalibrationTime;
+ }
+
+ // True only if the mag has calibration values stored
+ boolean HasMagCalibration() {
+ return MagCalibrated;
+ }
+
+ // Force the mag into the uncalibrated state
+ void ClearMagCalibration() {
+ MagCalibrated = false;
+ }
+
+ // These refer to reference points that associate mag readings with
+ // orientations
+ void ClearMagReferences() {
+ MagNumReferences = 0;
+ }
+
+ void SetMagRefDistance(float d) {
+ MagRefDistance = d;
+ }
+
+ float GetMagRefYaw() {
+ return MagRefYaw;
+ }
+
+ float GetYawErrorAngle() {
+ return YawErrorAngle;
+ }
+
+ void yawCorrect() {
+ // Yaw drift correction based on magnetometer data. This corrects
+ // the
+ // part of the drift that the accelerometer cannot handle. This will
+ // only work if the magnetometer has been enabled, calibrated, and a
+ // reference point has been set.
+
+ if (AngVL < maxAngVelLength) {
+ MagCondCount++;
+ } else {
+ MagCondCount = 0;
+ }
+
+ // Find, create, and utilize reference points for the magnetometer
+ // Need
+ // to be careful not to set reference points while there is
+ // significant
+ // tilt error
+ if ((MagCalibrated) && (RunningTime > 10.0f) && (TiltErrorAngle < 0.2f)) {
+ // if (MagNumReferences == 0) {
+ // setMagReference(Q, RawMag); // Use the current direction
+ // } else if (Q.Distance(MagRefQ) > MagRefDistance) {
+ // MagHasNearbyReference = false;
+ // float bestDist = 100000.0f;
+ // int bestNdx = 0;
+ // float dist;
+ // for (int i = 0; i < MagNumReferences; i++) {
+ // dist = Q.Distance(MagRefTableQ[i]);
+ // if (dist < bestDist) {
+ // bestNdx = i;
+ // bestDist = dist;
+ // }
+ // }
+ //
+ // if (bestDist < MagRefDistance) {
+ // MagHasNearbyReference = true;
+ // MagRefQ = MagRefTableQ[bestNdx];
+ // MagRefM = MagRefTableM[bestNdx];
+ // MagRefYaw = MagRefTableYaw[bestNdx];
+ // } else if (MagNumReferences < MagMaxReferences) {
+ // setMagReference(Q, RawMag);
+ // }
+ // }
+ }
+
+ YawCorrectionInProgress = false;
+ if (MagCalibrated && (RunningTime > 2.0f) && (MagCondCount >= magWindow)
+ && MagHasNearbyReference) {
+ // Use rotational invariance to bring reference mag value into
+ // global frame
+ Vector3f grefmag = MagRefQ.mult(getCalibratedMagValue(MagRefM));
+ // Bring current (averaged) mag reading into global frame
+ Vector3f gmag = Q.mult(getCalibratedMagValue(FRawMag.Mean()));
+ // Calculate the reference yaw in the global frame
+ float gryaw = (float) Math.atan2(grefmag.x, grefmag.z);
+ // Calculate the current yaw in the global frame
+ float gyaw = (float) Math.atan2(gmag.x, gmag.z);
+ // The difference between reference and current yaws is the
+ // perceived error
+ // Anglef YawErrorAngle = gyaw - gryaw;
+ //
+ // // LogText("Yaw error estimate: %f\n",YawErrorAngle.Get());
+ // // If the perceived error is large, keep count
+ // if ((YawErrorAngle.Abs() > yawErrorMax) &&
+ // (!YawCorrectionActivated)) {
+ // YawErrorCount++;
+ // }
+ // // After enough iterations of high perceived error, start the
+ // // correction process
+ // if (YawErrorCount > yawErrorCountLimit) {
+ // YawCorrectionActivated = true;
+ // }
+ // // If the perceived error becomes small, turn off the yaw
+ // // correction
+ // if ((YawErrorAngle.Abs() < yawErrorMin) &&
+ // YawCorrectionActivated) {
+ // YawCorrectionActivated = false;
+ // YawErrorCount = 0;
+ // }
+ //
+ // // Perform the actual yaw correction, due to previously
+ // // detected,
+ // // large yaw error
+ // if (YawCorrectionActivated) {
+ // YawCorrectionInProgress = true;
+ // // Incrementally "unyaw" by a small step size
+ // Q.multLocal(new Quaternion(-yawRotationStep *
+ // YawErrorAngle.Sign(), Vector3f.UNIT_Y, true));
+ // }
+ }
+ }
+
+ // *** Message Handler Logic
+
+ // // Notifies SensorFusion object about a new BodyFrame message from a
+ // sensor.
+ // // Should be called by user if not attaching to a sensor.
+ // // Default to current HMD orientation
+ // void setMagReference() {
+ // setMagReference(Q, RawMag);
+ // }
+
+ // // Writes the current calibration for a particular device to a device
+ // profile file
+ // // sensor - the sensor that was calibrated
+ // // cal_name - an optional name for the calibration or default if
+ // cal_name
+ // == NULL
+ // bool SensorFusion::SaveMagCalibration(const char* calibrationName)
+ // const
+ // {
+ // if (pSensor == NULL || !HasMagCalibration())
+ // return false;
+ //
+ // // A named calibration may be specified for calibration in different
+ // // environments, otherwise the default calibration is used
+ // if (calibrationName == NULL)
+ // calibrationName = "default";
+ //
+ // SensorInfo sinfo;
+ // pSensor->GetDeviceInfo(&sinfo);
+ //
+ // // Generate a mag calibration event
+ // JSON* calibration = JSON::CreateObject();
+ // // (hardcoded for now) the measurement and representation method
+ // calibration->AddStringItem("Version", "1.0");
+ // calibration->AddStringItem("Name", "default");
+ //
+ // // time stamp the calibration
+ // char time_str[64];
+ //
+ // #ifdef OVR_OS_WIN32
+ // struct tm caltime;
+ // localtime_s(&caltime, &MagCalibrationTime);
+ // strftime(time_str, 64, "%Y-%m-%d %H:%M:%S", &caltime);
+ // #else
+ // struct tm* caltime;
+ // caltime = localtime(&MagCalibrationTime);
+ // strftime(time_str, 64, "%Y-%m-%d %H:%M:%S", caltime);
+ // #endif
+ //
+ // calibration->AddStringItem("Time", time_str);
+ //
+ // // write the full calibration matrix
+ // Matrix4f calmat = GetMagCalibration();
+ // char matrix[128];
+ // int pos = 0;
+ // for (int r=0; r<4; r++)
+ // {
+ // for (int c=0; c<4; c++)
+ // {
+ // pos += (int)OVR_sprintf(matrix+pos, 128, "%g ", calmat.M[r][c]);
+ // }
+ // }
+ // calibration->AddStringItem("Calibration", matrix);
+ //
+ //
+ // String path = GetBaseOVRPath(true);
+ // path += "/Devices.json";
+ //
+ // // Look for a prexisting device file to edit
+ // Ptr<JSON> root = *JSON::Load(path);
+ // if (root)
+ // { // Quick sanity check of the file type and format before we parse
+ // it
+ // JSON* version = root->GetFirstItem();
+ // if (version && version->Name == "Oculus Device Profile Version")
+ // { // In the future I may need to check versioning to determine parse
+ // method
+ // }
+ // else
+ // {
+ // root->Release();
+ // root = NULL;
+ // }
+ // }
+ //
+ // JSON* device = NULL;
+ // if (root)
+ // {
+ // device = root->GetFirstItem(); // skip the header
+ // device = root->GetNextItem(device);
+ // while (device)
+ // { // Search for a previous calibration with the same name for this
+ // device
+ // // and remove it before adding the new one
+ // if (device->Name == "Device")
+ // {
+ // JSON* item = device->GetItemByName("Serial");
+ // if (item && item->Value == sinfo.SerialNumber)
+ // { // found an entry for this device
+ // item = device->GetNextItem(item);
+ // while (item)
+ // {
+ // if (item->Name == "MagCalibration")
+ // {
+ // JSON* name = item->GetItemByName("Name");
+ // if (name && name->Value == calibrationName)
+ // { // found a calibration of the same name
+ // item->RemoveNode();
+ // item->Release();
+ // break;
+ // }
+ // }
+ // item = device->GetNextItem(item);
+ // }
+ //
+ // // update the auto-mag flag
+ // item = device->GetItemByName("EnableYawCorrection");
+ // if (item)
+ // item->dValue = (double)EnableYawCorrection;
+ // else
+ // device->AddBoolItem("EnableYawCorrection", EnableYawCorrection);
+ //
+ // break;
+ // }
+ // }
+ //
+ // device = root->GetNextItem(device);
+ // }
+ // }
+ // else
+ // { // Create a new device root
+ // root = *JSON::CreateObject();
+ // root->AddStringItem("Oculus Device Profile Version", "1.0");
+ // }
+ //
+ // if (device == NULL)
+ // {
+ // device = JSON::CreateObject();
+ // device->AddStringItem("Product", sinfo.ProductName);
+ // device->AddNumberItem("ProductID", sinfo.ProductId);
+ // device->AddStringItem("Serial", sinfo.SerialNumber);
+ // device->AddBoolItem("EnableYawCorrection", EnableYawCorrection);
+ //
+ // root->AddItem("Device", device);
+ // }
+ //
+ // // Create and the add the new calibration event to the device
+ // device->AddItem("MagCalibration", calibration);
+ //
+ // return root->Save(path);
+ // }
+ //
+ // // Loads a saved calibration for the specified device from the device
+ // profile file
+ // // sensor - the sensor that the calibration was saved for
+ // // cal_name - an optional name for the calibration or the default if
+ // cal_name == NULL
+ // bool SensorFusion::LoadMagCalibration(const char* calibrationName)
+ // {
+ // if (pSensor == NULL)
+ // return false;
+ //
+ // // A named calibration may be specified for calibration in different
+ // // environments, otherwise the default calibration is used
+ // if (calibrationName == NULL)
+ // calibrationName = "default";
+ //
+ // SensorInfo sinfo;
+ // pSensor->GetDeviceInfo(&sinfo);
+ //
+ // String path = GetBaseOVRPath(true);
+ // path += "/Devices.json";
+ //
+ // // Load the device profiles
+ // Ptr<JSON> root = *JSON::Load(path);
+ // if (root == NULL)
+ // return false;
+ //
+ // // Quick sanity check of the file type and format before we parse it
+ // JSON* version = root->GetFirstItem();
+ // if (version && version->Name == "Oculus Device Profile Version")
+ // { // In the future I may need to check versioning to determine parse
+ // method
+ // }
+ // else
+ // {
+ // return false;
+ // }
+ //
+ // bool autoEnableCorrection = false;
+ //
+ // JSON* device = root->GetNextItem(version);
+ // while (device)
+ // { // Search for a previous calibration with the same name for this
+ // device
+ // // and remove it before adding the new one
+ // if (device->Name == "Device")
+ // {
+ // JSON* item = device->GetItemByName("Serial");
+ // if (item && item->Value == sinfo.SerialNumber)
+ // { // found an entry for this device
+ //
+ // JSON* autoyaw = device->GetItemByName("EnableYawCorrection");
+ // if (autoyaw)
+ // autoEnableCorrection = (autoyaw->dValue != 0);
+ //
+ // item = device->GetNextItem(item);
+ // while (item)
+ // {
+ // if (item->Name == "MagCalibration")
+ // {
+ // JSON* calibration = item;
+ // JSON* name = calibration->GetItemByName("Name");
+ // if (name && name->Value == calibrationName)
+ // { // found a calibration with this name
+ //
+ // time_t now;
+ // time(&now);
+ //
+ // // parse the calibration time
+ // time_t calibration_time = now;
+ // JSON* caltime = calibration->GetItemByName("Time");
+ // if (caltime)
+ // {
+ // const char* caltime_str = caltime->Value.ToCStr();
+ //
+ // tm ct;
+ // memset(&ct, 0, sizeof(tm));
+ //
+ // #ifdef OVR_OS_WIN32
+ // struct tm nowtime;
+ // localtime_s(&nowtime, &now);
+ // ct.tm_isdst = nowtime.tm_isdst;
+ // sscanf_s(caltime_str, "%d-%d-%d %d:%d:%d",
+ // &ct.tm_year, &ct.tm_mon, &ct.tm_mday,
+ // &ct.tm_hour, &ct.tm_min, &ct.tm_sec);
+ // #else
+ // struct tm* nowtime = localtime(&now);
+ // ct.tm_isdst = nowtime->tm_isdst;
+ // sscanf(caltime_str, "%d-%d-%d %d:%d:%d",
+ // &ct.tm_year, &ct.tm_mon, &ct.tm_mday,
+ // &ct.tm_hour, &ct.tm_min, &ct.tm_sec);
+ // #endif
+ // ct.tm_year -= 1900;
+ // ct.tm_mon--;
+ // calibration_time = mktime(&ct);
+ // }
+ //
+ // // parse the calibration matrix
+ // JSON* cal = calibration->GetItemByName("Calibration");
+ // if (cal)
+ // {
+ // const char* data_str = cal->Value.ToCStr();
+ // Matrix4f calmat;
+ // for (int r=0; r<4; r++)
+ // {
+ // for (int c=0; c<4; c++)
+ // {
+ // calmat.M[r][c] = (float)atof(data_str);
+ // while (data_str && *data_str != ' ')
+ // data_str++;
+ //
+ // if (data_str)
+ // data_str++;
+ // }
+ // }
+ //
+ // SetMagCalibration(calmat);
+ // MagCalibrationTime = calibration_time;
+ // EnableYawCorrection = autoEnableCorrection;
+ //
+ // return true;
+ // }
+ // }
+ // }
+ // item = device->GetNextItem(item);
+ // }
+ //
+ // break;
+ // }
+ // }
+ //
+ // device = root->GetNextItem(device);
+ // }
+ //
+ // return false;
+ // }
+ //
+ //
+
+ }
+};
diff --git a/Bindings/Java/src/main/java/org/saintandreas/oculus/hid/DisplayInfo.java b/Bindings/Java/src/main/java/org/saintandreas/oculus/hid/DisplayInfo.java
new file mode 100644
index 0000000..3063670
--- /dev/null
+++ b/Bindings/Java/src/main/java/org/saintandreas/oculus/hid/DisplayInfo.java
@@ -0,0 +1,75 @@
+package org.saintandreas.oculus.hid;
+
+import java.io.IOException;
+import java.nio.ByteBuffer;
+
+import org.saintandreas.input.hid.HidFeatureReport;
+
+import com.codeminders.hidapi.HIDDevice;
+
+/**
+ * DisplayInfo obtained from sensor; these values are used to report distortion
+ * settings and other coefficients. Older SensorDisplayInfo will have all zeros
+ *
+ */
+public final class DisplayInfo extends HidFeatureReport {
+ public static final byte FEATURE_ID = 9;
+ public static final int FEATURE_SIZE = 56;
+
+ public static final float SCALE = (1 / 1000000.f);
+ public static final int SIZE = 56;
+
+ public short commandId;
+ public byte distortion;
+ public short xres, yres;
+ public int xsize, ysize;
+ public int center;
+ public int sep;
+ public int[] zeye;
+ public float[] distortionCoefficients;
+
+ public DisplayInfo() {
+ super(FEATURE_ID, FEATURE_SIZE);
+ }
+
+ public DisplayInfo(HIDDevice device) throws IOException {
+ super(FEATURE_ID, FEATURE_SIZE, device);
+ }
+
+ @Override
+ protected void parse(ByteBuffer buffer) {
+ buffer.position(1);
+ commandId = buffer.getShort();
+ distortion = buffer.get();
+ xres = buffer.getShort();
+ yres = buffer.getShort();
+ xsize = buffer.getInt();
+ ysize = buffer.getInt();
+ center = buffer.getInt();
+ sep = buffer.getInt();
+ zeye = new int[2];
+ buffer.asIntBuffer().get(zeye);
+ buffer.position(buffer.position() + 8);
+ distortionCoefficients = new float[6];
+ buffer.asFloatBuffer().get(distortionCoefficients);
+ buffer.position(buffer.position() + 8);
+ }
+
+ @Override
+ protected void pack(ByteBuffer buffer) {
+ buffer.putShort(commandId);
+ buffer.put(distortion);
+ buffer.putShort(xres);
+ buffer.putShort(yres);
+ buffer.putInt(xsize);
+ buffer.putInt(ysize);
+ buffer.putInt(center);
+ buffer.putInt(sep);
+ for (int i = 0; i < 2; ++i) {
+ buffer.putInt(zeye[i]);
+ }
+ for (int i = 0; i < 6; ++i) {
+ buffer.putFloat(distortionCoefficients[i]);
+ }
+ }
+}
diff --git a/Bindings/Java/src/main/java/org/saintandreas/oculus/hid/KeepAlive.java b/Bindings/Java/src/main/java/org/saintandreas/oculus/hid/KeepAlive.java
new file mode 100644
index 0000000..80e24eb
--- /dev/null
+++ b/Bindings/Java/src/main/java/org/saintandreas/oculus/hid/KeepAlive.java
@@ -0,0 +1,41 @@
+package org.saintandreas.oculus.hid;
+
+import java.io.IOException;
+import java.nio.ByteBuffer;
+
+import org.saintandreas.input.hid.HidFeatureReport;
+
+import com.codeminders.hidapi.HIDDevice;
+
+public class KeepAlive extends HidFeatureReport {
+ public static final byte FEATURE_ID = 8;
+ public static final int FEATURE_SIZE = 5;
+
+ public short commandId = 0;
+ public short intervalMilliSeconds;
+
+ public KeepAlive() {
+ super(FEATURE_ID, FEATURE_SIZE);
+ }
+
+ public KeepAlive(HIDDevice device) throws IOException {
+ super(FEATURE_ID, FEATURE_SIZE, device);
+ }
+
+ public KeepAlive(short keepAliveMs) {
+ super(FEATURE_ID, FEATURE_SIZE);
+ this.intervalMilliSeconds = keepAliveMs;
+ }
+
+ @Override
+ protected void parse(ByteBuffer buffer) {
+ commandId = buffer.getShort();
+ intervalMilliSeconds = buffer.getShort();
+ }
+
+ @Override
+ protected void pack(ByteBuffer buffer) {
+ buffer.putShort(commandId);
+ buffer.putShort(intervalMilliSeconds);
+ }
+}
diff --git a/Bindings/Java/src/main/java/org/saintandreas/oculus/hid/SensorConfig.java b/Bindings/Java/src/main/java/org/saintandreas/oculus/hid/SensorConfig.java
new file mode 100644
index 0000000..4731ce5
--- /dev/null
+++ b/Bindings/Java/src/main/java/org/saintandreas/oculus/hid/SensorConfig.java
@@ -0,0 +1,59 @@
+package org.saintandreas.oculus.hid;
+
+import java.io.IOException;
+import java.nio.ByteBuffer;
+
+import org.saintandreas.input.hid.HidFeatureReport;
+
+import com.codeminders.hidapi.HIDDevice;
+
+public final class SensorConfig extends HidFeatureReport {
+ public static final byte FEATURE_ID = 2;
+ public static final int FEATURE_SIZE = 7;
+
+ public SensorConfig() {
+ super(FEATURE_ID, FEATURE_SIZE);
+ }
+
+ public SensorConfig(HIDDevice device) throws IOException {
+ super(FEATURE_ID, FEATURE_SIZE, device);
+ }
+
+ public enum Flag {
+ RawMode(0x01),
+ CallibrationTest(0x02),
+ UseCallibration(0x04),
+ AutoCallibration(0x08),
+ MotionKeepAlive(0x10),
+ CommandKeepAlive(0x20),
+ SensorCoordinates(0x40);
+
+ public final int value;
+
+ Flag(int value) {
+ this.value = value;
+ }
+ };
+
+ public short commandId;
+ public byte flags;
+ public short packetInterval;
+ public short keepAliveInterval;
+
+ @Override
+ protected void parse(ByteBuffer buffer) {
+ commandId = buffer.getShort();
+ flags = buffer.get();
+ packetInterval = buffer.get();
+ keepAliveInterval = buffer.getShort();
+ }
+
+ @Override
+ protected void pack(ByteBuffer buffer) {
+ buffer.putShort(commandId);
+ buffer.put(flags);
+ buffer.put((byte) packetInterval);
+ buffer.putShort(keepAliveInterval);
+ }
+
+}
diff --git a/Bindings/Java/src/main/java/org/saintandreas/oculus/hid/SensorRange.java b/Bindings/Java/src/main/java/org/saintandreas/oculus/hid/SensorRange.java
new file mode 100644
index 0000000..02fcb72
--- /dev/null
+++ b/Bindings/Java/src/main/java/org/saintandreas/oculus/hid/SensorRange.java
@@ -0,0 +1,52 @@
+package org.saintandreas.oculus.hid;
+
+import java.io.IOException;
+import java.nio.ByteBuffer;
+
+import org.saintandreas.input.hid.HidFeatureReport;
+
+import com.codeminders.hidapi.HIDDevice;
+
+/**
+ * Factor in the range conversion
+ *
+ */
+public final class SensorRange extends HidFeatureReport {
+ public static final byte FEATURE_ID = 4;
+ public static final int FEATURE_SIZE = 8;
+
+ public static final float EARTH_GEES_TO_METERS_PER_SECOND = 9.81f;
+ public static final float MILLIGAUSS_TO_GAUSS = 0.001f;
+
+ public short commandId;
+ // Meters per second squared
+ public short accelerationScale;
+ // Radians
+ public short gyroScale;
+ // Gauss
+ public short magScale;
+
+ public SensorRange() {
+ super(FEATURE_ID, FEATURE_SIZE);
+ }
+
+ public SensorRange(HIDDevice device) throws IOException {
+ super(FEATURE_ID, FEATURE_SIZE, device);
+ }
+
+ @Override
+ public void parse(ByteBuffer buffer) {
+ commandId = buffer.getShort();
+ accelerationScale = buffer.get();
+ gyroScale = buffer.getShort();
+ magScale = buffer.getShort();
+ }
+
+ @Override
+ public void pack(ByteBuffer buffer) {
+ buffer.putShort(commandId);
+ buffer.put((byte) accelerationScale);
+ buffer.putShort(gyroScale);
+ buffer.putShort(magScale);
+ }
+} \ No newline at end of file
diff --git a/Bindings/Java/src/main/java/org/saintandreas/oculus/tracker/TrackerMessage.java b/Bindings/Java/src/main/java/org/saintandreas/oculus/tracker/TrackerMessage.java
new file mode 100644
index 0000000..dbc590d
--- /dev/null
+++ b/Bindings/Java/src/main/java/org/saintandreas/oculus/tracker/TrackerMessage.java
@@ -0,0 +1,32 @@
+package org.saintandreas.oculus.tracker;
+
+import java.nio.ByteBuffer;
+import java.util.List;
+
+import com.google.common.collect.ImmutableList;
+import com.google.common.collect.Lists;
+
+public class TrackerMessage {
+ public final int type;
+ public final int sampleCount;
+ public final int timestamp;
+ public final int lastCommandId;
+ public final int temperature;
+ public final List<TrackerSample> samples;
+ public final TrackerVector mag;
+
+ public TrackerMessage(ByteBuffer buffer) {
+ type = buffer.get();
+ sampleCount = buffer.get() & 0xff;
+ timestamp = (buffer.getShort() & 0xffff);
+ lastCommandId = (buffer.getShort() & 0xffff);
+ temperature = (buffer.getShort() & 0xffff);
+ List<TrackerSample> samples = Lists.newArrayList();
+ for (int i = 0; i < 3; ++i) {
+ samples.add(new TrackerSample(buffer));
+ }
+ // this.samples = Lists.newArrayList();
+ this.samples = ImmutableList.copyOf(samples.subList(0, Math.min(3, sampleCount)));
+ mag = new TrackerVector(buffer, false);
+ }
+};
diff --git a/Bindings/Java/src/main/java/org/saintandreas/oculus/tracker/TrackerSample.java b/Bindings/Java/src/main/java/org/saintandreas/oculus/tracker/TrackerSample.java
new file mode 100644
index 0000000..b93f72f
--- /dev/null
+++ b/Bindings/Java/src/main/java/org/saintandreas/oculus/tracker/TrackerSample.java
@@ -0,0 +1,13 @@
+package org.saintandreas.oculus.tracker;
+
+import java.nio.ByteBuffer;
+
+public final class TrackerSample {
+ public final TrackerVector accel;
+ public final TrackerVector gyro;
+
+ public TrackerSample(ByteBuffer buffer) {
+ accel = new TrackerVector(buffer);
+ gyro = new TrackerVector(buffer);
+ }
+} \ No newline at end of file
diff --git a/Bindings/Java/src/main/java/org/saintandreas/oculus/tracker/TrackerVector.java b/Bindings/Java/src/main/java/org/saintandreas/oculus/tracker/TrackerVector.java
new file mode 100644
index 0000000..ac360ac
--- /dev/null
+++ b/Bindings/Java/src/main/java/org/saintandreas/oculus/tracker/TrackerVector.java
@@ -0,0 +1,49 @@
+package org.saintandreas.oculus.tracker;
+
+import java.nio.ByteBuffer;
+
+/**
+ * Encapsulates the integer vectors provided by the Oculus Rift. These are either encoded as short values, or
+ * as n
+ *
+ * @author [email protected]
+ *
+ */
+public final class TrackerVector {
+ private static final int BITS = 21;
+ private static final int MASK = 1 << (BITS - 1);
+
+ // Sign extension trick. The values are packed as 21 bit signed integers
+ // So we need to extend the sign at bit 21 all to way out to bit 32 for
+ // a Java int. See http://graphics.stanford.edu/~seander/bithacks.html#VariableSignExtend
+ private static int extend(int x) {
+ return (x ^ MASK) - MASK;
+ }
+
+ public final int x;
+ public final int y;
+ public final int z;
+
+ public TrackerVector(ByteBuffer buffer) {
+ this(buffer, true);
+ }
+
+ public TrackerVector(ByteBuffer buffer, boolean packed) {
+ if (packed) {
+ byte[] bs = new byte[8];
+ buffer.get(bs);
+ x = extend(((bs[0] & 0xff) << 13) | ((bs[1] & 0xff) << 5) | ((bs[2] & 0xf8) >> 3));
+ y = extend(((bs[2] & 0x07) << 18) | ((bs[3] & 0xff) << 10) | ((bs[4] & 0xff) << 2) | ((bs[5] & 0xc0) >> 6));
+ z = extend(((bs[5] & 0x3f) << 15) | ((bs[6] & 0xff) << 7) | ((bs[7] & 0xff) >> 1));
+ } else {
+ x = buffer.getShort();
+ y = buffer.getShort();
+ z = buffer.getShort();
+ }
+ }
+
+ @Override
+ public String toString() {
+ return "X: " + x + ", Y: " + y + ", Z: " + z;
+ }
+} \ No newline at end of file
diff --git a/Bindings/Java/src/test/java/org/saintandreas/input/oculus/RiftTest.java b/Bindings/Java/src/test/java/org/saintandreas/input/oculus/RiftTest.java
new file mode 100644
index 0000000..e5ebc55
--- /dev/null
+++ b/Bindings/Java/src/test/java/org/saintandreas/input/oculus/RiftTest.java
@@ -0,0 +1,81 @@
+package org.saintandreas.input.oculus;
+
+import java.io.IOException;
+
+import org.codehaus.jackson.map.ObjectMapper;
+import org.codehaus.jackson.map.ObjectWriter;
+import org.saintandreas.oculus.RiftTracker;
+import org.saintandreas.oculus.hid.DisplayInfo;
+import org.saintandreas.oculus.hid.SensorConfig;
+import org.saintandreas.oculus.hid.SensorRange;
+import org.saintandreas.oculus.tracker.TrackerMessage;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
+
+import com.codeminders.hidapi.HIDDevice;
+import com.google.common.base.Predicate;
+
+public class RiftTest {
+ private static final Logger LOG = LoggerFactory.getLogger(RiftTest.class);
+
+ public static void main(String... args) throws IOException, InterruptedException {
+ final ObjectMapper mapper = new ObjectMapper();
+ final ObjectWriter writer = mapper.writerWithDefaultPrettyPrinter();
+ RiftTracker tracker = RiftTracker.getInstance();
+ DisplayInfo di = tracker.getDisplayInfo();
+ LOG.info(writer.writeValueAsString(di));
+ SensorConfig sc = tracker.getSensorConfig();
+ LOG.info(writer.writeValueAsString(sc));
+ SensorRange sr = tracker.getSensorRange();
+ LOG.info(writer.writeValueAsString(sr));
+ RiftTracker.startListening(new Predicate<TrackerMessage>() {
+ @Override
+ public boolean apply(TrackerMessage message) {
+ try {
+ // Just enough data to see we're getting stuff
+ LOG.info("Sample count: " + message.sampleCount);
+ LOG.info("Sample 0: " + mapper.writeValueAsString(message.samples.get(0)));
+ } catch (Exception e) {
+ LOG.error("Failed to serialize sample data", e);
+ }
+ return true;
+ }
+ });
+ }
+
+ /**
+ * This is a tool I used to fix my display info report after I accidentally
+ * trashed it by runnig example code here:
+ * http://lxr.free-electrons.com/source/samples/hidraw/hid-example.c -- note
+ * the sample code writes to feature report 9 without actually doing any
+ * verification that that's a good idea.
+ *
+ * No guaratee that this will not destroy your device. No warranty is
+ * implied or provided.
+ *
+ * @param device
+ * @throws IOException
+ */
+ public static void fixDisplayInfo(HIDDevice device) throws IOException {
+ DisplayInfo di = new DisplayInfo();
+ // These values are from an earlier dump of my display Info. I have no
+ // idea if they're valid.
+ di.distortion = 1;
+ di.xres = 1280;
+ di.yres = 800;
+ di.xsize = 149760;
+ di.ysize = 93600;
+ di.center = 46800;
+ di.sep = 63500;
+ di.zeye = new int[] { 49800, 49800 };
+ di.distortionCoefficients = new float[] {
+ Float.NaN,
+ 1.5946803e-27f,
+ Float.NaN,
+ 3.4119901E16f,
+ 4.6665167E-38f,
+ -9.6494493E15f
+ };
+ di.write(device);
+ }
+}