Small VR Bridge refactoring

Added support for new trackers notification in VR Server
Refactor trackers adjustments, fix adjustments bugs, can have different adjustments types (full, yaw, anything in the future)
Fixed multiple problems with skeleton computation, added neck node
Added support for IMU trackers update rate calculation

src/main/java/io/eiren/vr/VRServer.java

@@ -10,6 +10,7 @@ import java.util.List;
 import java.util.Map;
 import java.util.Queue;
 import java.util.concurrent.LinkedBlockingQueue;
+import java.util.function.Consumer;
 
 import essentia.util.ann.ThreadSafe;
 import essentia.util.ann.ThreadSecure;
@@ -34,6 +35,7 @@ public class VRServer extends Thread {
 	private final Map<String, TrackerConfig> configuration = new HashMap<>();
 	public final YamlFile config = new YamlFile();
 	public final HMDTracker hmdTracker;
+	private final List<Consumer<Tracker>> newTrackersConsumers = new FastList<>();
 	
 	public VRServer() {
 		super("VRServer");
@@ -75,6 +77,16 @@ public class VRServer extends Thread {
 			}
 		}
 	}
+	
+	public void addNewTrackerConsumer(Consumer<Tracker> consumer) {
+		synchronized(newTrackersConsumers) {
+			newTrackersConsumers.add(consumer);
+		}
+		synchronized(trackers) {
+			for(int i = 0; i < trackers.size(); ++i)
+				consumer.accept(trackers.get(i));
+		}
+	}
 
 	@ThreadSafe
 	public void saveConfig() {
@@ -151,6 +163,10 @@ public class VRServer extends Thread {
 			trackers.add(tracker);
 		}
 		autoAssignTracker(tracker);
+		synchronized(newTrackersConsumers) {
+			for(int i = 0; i < newTrackersConsumers.size(); ++i)
+				newTrackersConsumers.get(i).accept(tracker);
+		}
 	}
 	
 	public void calibrate(Tracker tracker) {

src/main/java/io/eiren/vr/bridge/NamedPipeVRBridge.java

@@ -32,7 +32,6 @@ public class NamedPipeVRBridge extends Thread implements VRBridge {
 	private final HMDTracker hmd;
 	private final List<Pipe> trackerPipes;
 	private final List<? extends Tracker> shareTrackers;
-	protected VRBridgeState bridgeState = VRBridgeState.NOT_STARTED;
 	
 	public NamedPipeVRBridge(HMDTracker hmd, List<? extends Tracker> shareTrackers) {
 		super("Named Pipe VR Bridge");
@@ -41,91 +40,88 @@ public class NamedPipeVRBridge extends Thread implements VRBridge {
 		this.trackerPipes = new FastList<>(shareTrackers.size());
 	}
 	
-	@Override
-	public VRBridgeState getBridgeState() {
-		return bridgeState;
-	}
-	
 	@Override
 	public void run() {
 		try {
 			createPipes();
-			bridgeState = VRBridgeState.STARTED;
 			while(true) {
-				if(bridgeState == VRBridgeState.STARTED) {
-					if(hmdPipe != null && hmdPipe.state == PipeState.CREATED) {
-						if(tryOpeningPipe(hmdPipe))
-							initHMDPipe(hmdPipe);
-					}
+				waitForPipesToOpen();
+				if(areAllPipesOpen()) {
+					boolean hmdUpdated = updateHMD(); // Update at HMDs frequency
 					for(int i = 0; i < trackerPipes.size(); ++i) {
-						Pipe trackerPipe = trackerPipes.get(i);
-						if(trackerPipe.state == PipeState.CREATED)
-							if(tryOpeningPipe(trackerPipe))
-								initTrackerPipe(trackerPipe, i);
+						updateTracker(i, hmdUpdated);
 					}
-					if(areAllPipesOpen()) {
-						bridgeState = VRBridgeState.CONNECTED;
-						LogManager.log.info("[VRBridge] All pipes are connected!");
-					} else {
-						Thread.sleep(200L);
-					}
-				} else {
-					// TODO Handle pipes disconnect, reset state and prepare to send hello again
-					if(updateHMD()) { // Update at HMDs frequency
-						for(int i = 0; i < trackerPipes.size(); ++i) {
-							updateTracker(i);
-						}
-					} else {
+					if(!hmdUpdated) {
 						Thread.sleep(5); // Up to 200Hz
 					}
 				}
 			}
 		} catch(Exception e) {
 			e.printStackTrace();
-			bridgeState = VRBridgeState.ERROR;
+		}
+	}
+	
+	private void waitForPipesToOpen() {
+		if(hmdPipe.state == PipeState.CREATED) {
+			if(tryOpeningPipe(hmdPipe))
+				initHMDPipe(hmdPipe);
+		}
+		for(int i = 0; i < trackerPipes.size(); ++i) {
+			Pipe trackerPipe = trackerPipes.get(i);
+			if(trackerPipe.state == PipeState.CREATED) {
+				if(tryOpeningPipe(trackerPipe))
+					initTrackerPipe(trackerPipe, i);
+			}
 		}
 	}
 	
 	public boolean updateHMD() {
-		IntByReference bytesAvailable = new IntByReference(0);
-		if(Kernel32.INSTANCE.PeekNamedPipe(hmdPipe.pipeHandle, null, 0, null, bytesAvailable, null)) {
-			if(bytesAvailable.getValue() > 0) {
-				if(Kernel32.INSTANCE.ReadFile(hmdPipe.pipeHandle, buffer, buffer.length, bytesAvailable, null)) {
-					String str = new String(buffer, 0, bytesAvailable.getValue() - 1, ASCII);
-					String[] split = str.split("\n")[0].split(" ");
-					try {
-						double x = Double.parseDouble(split[0]);
-						double y = Double.parseDouble(split[1]);
-						double z = Double.parseDouble(split[2]);
-						double qw = Double.parseDouble(split[3]);
-						double qx = Double.parseDouble(split[4]);
-						double qy = Double.parseDouble(split[5]);
-						double qz = Double.parseDouble(split[6]);
-						
-						hmd.position.set((float) x, (float) y, (float) z);
-						hmd.rotation.set((float) qx, (float) qy, (float) qz, (float) qw);
-					} catch(NumberFormatException e) {
-						e.printStackTrace();
+		if(hmdPipe.state == PipeState.OPEN) {
+			IntByReference bytesAvailable = new IntByReference(0);
+			if(Kernel32.INSTANCE.PeekNamedPipe(hmdPipe.pipeHandle, null, 0, null, bytesAvailable, null)) {
+				if(bytesAvailable.getValue() > 0) {
+					if(Kernel32.INSTANCE.ReadFile(hmdPipe.pipeHandle, buffer, buffer.length, bytesAvailable, null)) {
+						String str = new String(buffer, 0, bytesAvailable.getValue() - 1, ASCII);
+						String[] split = str.split("\n")[0].split(" ");
+						try {
+							double x = Double.parseDouble(split[0]);
+							double y = Double.parseDouble(split[1]);
+							double z = Double.parseDouble(split[2]);
+							double qw = Double.parseDouble(split[3]);
+							double qx = Double.parseDouble(split[4]);
+							double qy = Double.parseDouble(split[5]);
+							double qz = Double.parseDouble(split[6]);
+							
+							hmd.position.set((float) x, (float) y, (float) z);
+							hmd.rotation.set((float) qx, (float) qy, (float) qz, (float) qw);
+						} catch(NumberFormatException e) {
+							e.printStackTrace();
+						}
 					}
+					return true;
 				}
-				return true;
 			}
 		}
 		return false;
 	}
 	
-	public void updateTracker(int trackerId) {
-		sbBuffer.setLength(0);
+	public void updateTracker(int trackerId, boolean hmdUpdated) {
 		Tracker sensor = shareTrackers.get(trackerId);
-		sensor.getPosition(vBuffer);
-		sensor.getRotation(qBuffer);
-		sbBuffer.append(vBuffer.x).append(' ').append(vBuffer.y).append(' ').append(vBuffer.z).append(' ');
-		sbBuffer.append(qBuffer.getW()).append(' ').append(qBuffer.getX()).append(' ').append(qBuffer.getY()).append(' ').append(qBuffer.getZ()).append('\n');
-		String str = sbBuffer.toString();
-		System.arraycopy(str.getBytes(ASCII), 0, buffer, 0, str.length());
-		buffer[str.length()] = '\0';
-		IntByReference lpNumberOfBytesWritten = new IntByReference(0);
-		Kernel32.INSTANCE.WriteFile(trackerPipes.get(trackerId).pipeHandle, buffer, str.length() + 1, lpNumberOfBytesWritten, null);
+		if(sensor.getStatus().sendData) {
+			Pipe trackerPipe = trackerPipes.get(trackerId);
+			if(hmdUpdated && trackerPipe.state == PipeState.OPEN) {
+				sbBuffer.setLength(0);
+				sensor.getPosition(vBuffer);
+				sensor.getRotation(qBuffer);
+				sbBuffer.append(vBuffer.x).append(' ').append(vBuffer.y).append(' ').append(vBuffer.z).append(' ');
+				sbBuffer.append(qBuffer.getW()).append(' ').append(qBuffer.getX()).append(' ').append(qBuffer.getY()).append(' ').append(qBuffer.getZ()).append('\n');
+				String str = sbBuffer.toString();
+				System.arraycopy(str.getBytes(ASCII), 0, buffer, 0, str.length());
+				buffer[str.length()] = '\0';
+				IntByReference lpNumberOfBytesWritten = new IntByReference(0);
+				Kernel32.INSTANCE.WriteFile(trackerPipe.pipeHandle, buffer, str.length() + 1, lpNumberOfBytesWritten, null);
+			}
+		}
 	}
 	
 	private void initHMDPipe(Pipe pipe) {
@@ -150,6 +146,8 @@ public class NamedPipeVRBridge extends Thread implements VRBridge {
 			LogManager.log.info("[VRBridge] Pipe " + pipe.name + " is open");
 			return true;
 		}
+		
+		LogManager.log.info("[VRBridge] Error connecting to pipe " + pipe.name + ": " + Kernel32.INSTANCE.GetLastError());
 		return false;
 	}
 	

src/main/java/io/eiren/vr/bridge/VRBridge.java

@@ -2,12 +2,4 @@ package io.eiren.vr.bridge;
 
 public interface VRBridge {
 	
-	public VRBridgeState getBridgeState();
-	
-	public static enum VRBridgeState {
-		NOT_STARTED,
-		STARTED,
-		CONNECTED,
-		ERROR
-	}
 }

src/main/java/io/eiren/vr/processor/HumanPoseProcessor.java

@@ -6,10 +6,11 @@ import java.util.List;
 import java.util.Map;
 
 import com.jme3.math.Quaternion;
-import com.jme3.math.Vector3f;
 
 import essentia.util.collections.FastList;
 import io.eiren.vr.VRServer;
+import io.eiren.vr.trackers.AdjustedTracker;
+import io.eiren.vr.trackers.AdjustedYawTracker;
 import io.eiren.vr.trackers.HMDTracker;
 import io.eiren.vr.trackers.Tracker;
 import io.eiren.vr.trackers.TrackerConfig;
@@ -46,7 +47,7 @@ public class HumanPoseProcessor {
 	}
 	
 	private void addTracker(Tracker tracker, TrackerBodyPosition position) {
-		AdjustedTracker tt = new AdjustedTracker(tracker, position);
+		AdjustedTracker tt = new AdjustedYawTracker(tracker);
 		
 		TrackerConfig config = server.getTrackerConfig(tt);
 		if(config.adjustment != null)
@@ -74,44 +75,34 @@ public class HumanPoseProcessor {
 			if(skeleton instanceof HumanSekeletonWithLegs) {
 				return; // Proper skeleton applied
 			}
+			disconnectAllTrackers();
 			skeleton = new HumanSekeletonWithLegs(server, trackers, computedTrackers);
 		} else {
 			if(skeleton instanceof HumanSkeleonWithWaist) {
 				return; // Proper skeleton applied
 			}
+			disconnectAllTrackers();
 			skeleton = new HumanSkeleonWithWaist(server, trackers.get(TrackerBodyPosition.WAIST), computedTrackers);
 		}
 	}
 	
+	private void disconnectAllTrackers() {
+		for(int i = 0; i < computedTrackers.size(); ++i) {
+			computedTrackers.get(i).setStatus(TrackerStatus.DISCONNECTED);
+		}
+	}
+	
 	public void resetTrackers() {
-		Quaternion sensorRotation = new Quaternion();
 		Quaternion hmdRotation = new Quaternion();
-		Quaternion targetTrackerRotation = new Quaternion();
 		hmd.getRotation(hmdRotation);
-
-		// Adjust only yaw rotation
-		Vector3f hmdFront = new Vector3f(0, 0, 1);
-		hmdRotation.multLocal(hmdFront);
-		hmdFront.multLocal(1, 0, 1).normalizeLocal();
-		//hmdRotation.lookAt(hmdFront, Vector3f.UNIT_Y);
 		
 		Iterator<AdjustedTracker> iterator = trackers.values().iterator();
 		while(iterator.hasNext()) {
 			AdjustedTracker tt = iterator.next();
-			tt.tracker.getRotation(sensorRotation);
-
-			// Adjust only yaw rotation
-			Vector3f sensorFront = new Vector3f(0, 0, 1);
-			sensorRotation.multLocal(sensorFront);
-			sensorFront.multLocal(1, 0, 1).normalizeLocal();
-			//sensorRotation.lookAt(sensorFront, Vector3f.UNIT_Y);
-			
-			
-			tt.position.baseRotation.mult(hmdRotation, targetTrackerRotation);
-			tt.adjustment.set(sensorRotation).inverseLocal().multLocal(targetTrackerRotation);
+			tt.adjust(hmdRotation);
 			
 			TrackerConfig config = server.getTrackerConfig(tt);
-			config.adjustment = new Quaternion(tt.adjustment);
+			tt.saveAdjustment(config);
 		}
 	}
 	
@@ -119,37 +110,4 @@ public class HumanPoseProcessor {
 		if(skeleton != null)
 			skeleton.updatePose();
 	}
-	
-	private static class AdjustedTracker implements Tracker {
-		public final Tracker tracker;
-		public final Quaternion adjustment = new Quaternion();
-		public final TrackerBodyPosition position;
-		
-		public AdjustedTracker(Tracker tracker, TrackerBodyPosition position) {
-			this.tracker = tracker;
-			this.position = position;
-		}
-		
-		@Override
-		public boolean getRotation(Quaternion store) {
-			tracker.getRotation(store);
-			adjustment.mult(store, store);
-			return true;
-		}
-
-		@Override
-		public boolean getPosition(Vector3f store) {
-			return tracker.getPosition(store);
-		}
-
-		@Override
-		public String getName() {
-			return tracker.getName() + "/adj";
-		}
-
-		@Override
-		public TrackerStatus getStatus() {
-			return tracker.getStatus();
-		}
-	}
 }

src/main/java/io/eiren/vr/processor/HumanSekeletonWithLegs.java

@@ -5,6 +5,7 @@ import java.util.Map;
 
 import io.eiren.vr.VRServer;
 import io.eiren.vr.trackers.Tracker;
+import io.eiren.vr.trackers.TrackerStatus;
 
 public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 	
@@ -15,16 +16,16 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 	protected final Tracker rightAnkleTracker;
 	protected final ComputedHumanPoseTracker computedRightAnkleTracker;
 	
-	protected final TransformNode leftLegNode = new TransformNode();
+	protected final TransformNode leftHipNode = new TransformNode();
 	protected final TransformNode leftKneeNode = new TransformNode();
 	protected final TransformNode leftAnkleNode = new TransformNode();
-	protected final TransformNode rightLegNode = new TransformNode();
+	protected final TransformNode rightHipNode = new TransformNode();
 	protected final TransformNode rightKneeNode = new TransformNode();
 	protected final TransformNode rightAnkleNode = new TransformNode();
 	
 	protected float hipsWidth = 0.3f;
-	protected float kneeLength = 0.5f;
-	protected float ankleLength = 0.4f;
+	protected float hipLength = 0.86f;
+	protected float ankleLength = 0.42f;
 
 	public HumanSekeletonWithLegs(VRServer server, Map<TrackerBodyPosition, ? extends Tracker> trackers, List<ComputedHumanPoseTracker> computedTrackers) {
 		super(server, trackers.get(TrackerBodyPosition.WAIST), computedTrackers);
@@ -43,18 +44,20 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 		}
 		computedLeftAnkleTracker = lat;
 		computedRightAnkleTracker = rat;
+		lat.setStatus(TrackerStatus.OK);
+		rat.setStatus(TrackerStatus.OK);
 		
-		waistNode.attachChild(leftLegNode);
-		leftLegNode.localTransform.setTranslation(hipsWidth / 2, 0, 0);
+		waistNode.attachChild(leftHipNode);
+		leftHipNode.localTransform.setTranslation(hipsWidth / 2, 0, 0);
 		
-		waistNode.attachChild(rightLegNode);
-		rightLegNode.localTransform.setTranslation(-hipsWidth / 2, 0, 0);
+		waistNode.attachChild(rightHipNode);
+		rightHipNode.localTransform.setTranslation(-hipsWidth / 2, 0, 0);
 		
-		leftLegNode.attachChild(leftKneeNode);
-		leftKneeNode.localTransform.setTranslation(0, -kneeLength, 0);
+		leftHipNode.attachChild(leftKneeNode);
+		leftKneeNode.localTransform.setTranslation(0, -hipLength, 0);
 		
-		rightLegNode.attachChild(rightKneeNode);
-		rightKneeNode.localTransform.setTranslation(0, -kneeLength, 0);
+		rightHipNode.attachChild(rightKneeNode);
+		rightKneeNode.localTransform.setTranslation(0, -hipLength, 0);
 		
 		leftKneeNode.attachChild(leftAnkleNode);
 		leftAnkleNode.localTransform.setTranslation(0, -ankleLength, 0);
@@ -67,10 +70,10 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 	public void updateLocalTransforms() {
 		super.updateLocalTransforms();
 		leftLegTracker.getRotation(qBuf);
-		leftLegNode.localTransform.setRotation(qBuf);
+		leftHipNode.localTransform.setRotation(qBuf);
 		
 		rightLegTracker.getRotation(qBuf);
-		rightLegNode.localTransform.setRotation(qBuf);
+		rightHipNode.localTransform.setRotation(qBuf);
 		
 		leftAnkleTracker.getRotation(qBuf);
 		leftKneeNode.localTransform.setRotation(qBuf);

src/main/java/io/eiren/vr/processor/HumanSkeleonWithWaist.java

@@ -8,6 +8,7 @@ import com.jme3.math.Vector3f;
 import io.eiren.vr.VRServer;
 import io.eiren.vr.trackers.HMDTracker;
 import io.eiren.vr.trackers.Tracker;
+import io.eiren.vr.trackers.TrackerStatus;
 
 public class HumanSkeleonWithWaist extends HumanSkeleton {
 	
@@ -17,10 +18,13 @@ public class HumanSkeleonWithWaist extends HumanSkeleton {
 	protected final Tracker wasitTracker;
 	protected final HMDTracker hmdTracker;
 	protected final ComputedHumanPoseTracker computedWaistTracker;
-	protected float waistDistance = 0.63f;
-	protected float waistSwingMultiplier = 1f;
 	protected final TransformNode hmdNode = new TransformNode();
+	protected final TransformNode neckNode = new TransformNode();
 	protected final TransformNode waistNode = new TransformNode();
+	
+	protected float waistDistance = 0.72f;
+	protected float waistSwingMultiplier = 1f;
+	protected float neckLength = 0.2f;
 
 	public HumanSkeleonWithWaist(VRServer server, Tracker waistTracker, List<ComputedHumanPoseTracker> computedTrackers) {
 		this.wasitTracker = waistTracker;
@@ -32,11 +36,15 @@ public class HumanSkeleonWithWaist extends HumanSkeleton {
 				cwt = t;
 		}
 		computedWaistTracker = cwt;
+		cwt.setStatus(TrackerStatus.OK);
 		waistDistance = server.config.getFloat("body.waistDistance", waistDistance);
 		waistSwingMultiplier = server.config.getFloat("body.waistSwingMultiplier", waistSwingMultiplier);
 		// Build skeleton
-		hmdNode.attachChild(waistNode);
-		waistNode.localTransform.setTranslation(0, -waistDistance, 0);
+		hmdNode.attachChild(neckNode);
+		waistNode.localTransform.setTranslation(0, -neckLength, 0);
+		
+		neckNode.attachChild(waistNode);
+		waistNode.localTransform.setTranslation(0, -waistDistance + neckLength, 0);
 	}
 	
 	@Override
@@ -47,14 +55,17 @@ public class HumanSkeleonWithWaist extends HumanSkeleton {
 	}
 	
 	protected void updateLocalTransforms() {
+		hmdTracker.getPosition(vBuf);
+		hmdTracker.getRotation(qBuf);
+		hmdNode.localTransform.setTranslation(vBuf);
+		hmdNode.localTransform.setRotation(qBuf);
+		
 		wasitTracker.getRotation(qBuf);
 		if(waistSwingMultiplier != 1.0) {
 			// TODO : Adjust waist swing if swing multiplier != 0
 		}
 		
-		hmdTracker.getPosition(vBuf);
-		hmdNode.localTransform.setTranslation(vBuf);
-		hmdNode.localTransform.setRotation(qBuf);
+		neckNode.localTransform.setRotation(qBuf);
 		waistNode.localTransform.setRotation(qBuf);
 	}
 	

src/main/java/io/eiren/vr/trackers/AdjustedTracker.java

@@ -0,0 +1,47 @@
+package io.eiren.vr.trackers;
+
+import com.jme3.math.Quaternion;
+import com.jme3.math.Vector3f;
+
+public class AdjustedTracker implements Tracker {
+	
+	public final Tracker tracker;
+	public final Quaternion adjustment = new Quaternion();
+	
+	public AdjustedTracker(Tracker tracker) {
+		this.tracker = tracker;
+	}
+	
+	public void saveAdjustment(TrackerConfig cfg) {
+		cfg.adjustment = new Quaternion(adjustment);
+	}
+	
+	public void adjust(Quaternion reference) {
+		Quaternion sensorRotation = new Quaternion();
+		tracker.getRotation(sensorRotation);
+		
+		adjustment.set(sensorRotation).inverseLocal().multLocal(reference);
+	}
+	
+	@Override
+	public boolean getRotation(Quaternion store) {
+		tracker.getRotation(store);
+		adjustment.mult(store, store);
+		return true;
+	}
+
+	@Override
+	public boolean getPosition(Vector3f store) {
+		return tracker.getPosition(store);
+	}
+
+	@Override
+	public String getName() {
+		return tracker.getName() + "/adj";
+	}
+
+	@Override
+	public TrackerStatus getStatus() {
+		return tracker.getStatus();
+	}
+}

src/main/java/io/eiren/vr/trackers/AdjustedYawTracker.java

@@ -0,0 +1,33 @@
+package io.eiren.vr.trackers;
+
+import com.jme3.math.Quaternion;
+import com.jme3.math.Vector3f;
+
+public class AdjustedYawTracker extends AdjustedTracker {
+
+	public AdjustedYawTracker(Tracker tracker) {
+		super(tracker);
+	}
+	
+	@Override
+	public void adjust(Quaternion reference) {
+		Quaternion targetTrackerRotation = new Quaternion(reference);
+		
+		// Use only yaw rotation
+		Vector3f hmdFront = new Vector3f(0, 0, 1);
+		targetTrackerRotation.multLocal(hmdFront);
+		hmdFront.multLocal(1, 0, 1).normalizeLocal();
+		targetTrackerRotation.lookAt(hmdFront, Vector3f.UNIT_Y);
+		
+		Quaternion sensorRotation = new Quaternion();
+		tracker.getRotation(sensorRotation);
+		
+		// Adjust only yaw rotation
+		Vector3f sensorFront = new Vector3f(0, 0, 1);
+		sensorRotation.multLocal(sensorFront);
+		sensorFront.multLocal(1, 0, 1).normalizeLocal();
+		sensorRotation.lookAt(sensorFront, Vector3f.UNIT_Y);
+		
+		adjustment.set(sensorRotation).inverseLocal().multLocal(targetTrackerRotation);
+	}
+}

src/main/java/io/eiren/vr/trackers/CalibratingTracker.java

@@ -0,0 +1,6 @@
+package io.eiren.vr.trackers;
+
+public interface CalibratingTracker {
+	
+	public void startCalibration();
+}

src/main/java/io/eiren/vr/trackers/IMUTracker.java

@@ -3,7 +3,9 @@ package io.eiren.vr.trackers;
 import com.jme3.math.Quaternion;
 import com.jme3.math.Vector3f;
 
-public class IMUTracker implements Tracker {
+import io.eiren.util.BufferedTimer;
+
+public class IMUTracker implements Tracker, CalibratingTracker {
 	
 	public final Vector3f gyroVector = new Vector3f();
 	public final Vector3f accelVector = new Vector3f();
@@ -14,6 +16,8 @@ public class IMUTracker implements Tracker {
 	protected final String name;
 	protected final TrackersUDPServer server;
 	
+	protected BufferedTimer timer = new BufferedTimer(1f);
+	
 	public IMUTracker(String name, TrackersUDPServer server) {
 		this.name = name;
 		this.server = server;
@@ -45,7 +49,16 @@ public class IMUTracker implements Tracker {
 		this.status = status;
 	}
 	
+	@Override
 	public void startCalibration() {
-		
+		server.sendCalibrationCommand(this);
+	}
+	
+	public float getTPS() {
+		return timer.getAverageFPS();
+	}
+	
+	public void dataTick() {
+		timer.update();
 	}
 }

src/main/java/io/eiren/vr/trackers/TrackerStatus.java

@@ -2,8 +2,15 @@ package io.eiren.vr.trackers;
 
 public enum TrackerStatus {
 	
-	DISCONNECTED,
-	OK,
-	BUSY,
-	ERROR;
+	DISCONNECTED(false),
+	OK(true),
+	BUSY(true),
+	ERROR(false),
+	;
+	
+	public final boolean sendData;
+	
+	private TrackerStatus(boolean sendData) {
+		this.sendData = sendData;
+	}
 }

src/main/java/io/eiren/vr/trackers/TrackersUDPServer.java

@@ -25,13 +25,17 @@ import io.eiren.util.Util;
  * Recieves trackers data by UDP using extended owoTrack protocol.
  */
 public class TrackersUDPServer extends Thread {
+
+	/**
+	 * Change between IMU axises and OpenGL/SteamVR axises
+	 */
+	private static final Quaternion offset = new Quaternion().fromAngleAxis(-FastMath.HALF_PI, Vector3f.UNIT_X);
 	
 	private static final byte[] HANDSHAKE_BUFFER = new byte[64];
 	private static final byte[] KEEPUP_BUFFER = new byte[64];
 	private static final byte[] CALIBRATION_BUFFER = new byte[64];
 
 	private final Quaternion buf = new Quaternion();
-	private final byte[] sendBuffer = new byte[64];
 
 	private final List<TrackerConnection> trackers = new FastList<>();
 	private final Map<SocketAddress, TrackerConnection> trackersMap = new HashMap<>();
@@ -151,7 +155,6 @@ public class TrackersUDPServer extends Thread {
         socket.send(new DatagramPacket(HANDSHAKE_BUFFER, HANDSHAKE_BUFFER.length, handshakePacket.getAddress(), handshakePacket.getPort()));
 	}
 	
-	private static final Quaternion offset = new Quaternion().fromAngleAxis(-FastMath.HALF_PI, Vector3f.UNIT_X);
 	
 	@Override
 	public void run() {
@@ -180,11 +183,9 @@ public class TrackersUDPServer extends Thread {
 						bb.getLong();
 						stopCalibration(sensor);
 						buf.set(bb.getFloat(), bb.getFloat(), bb.getFloat(), bb.getFloat());
-						//buf.set(buf.getY(), buf.getZ(), buf.getY(), buf.getW()); // Change from sensor rotation to OpenGL/SteamVR rotation
-						//Quaternion q3 = new Quaternion().fromAngleAxis(FastMath.PI, Vector3f.UNIT_Y).multLocal(q2);
 						offset.mult(buf, buf);
 						sensor.tracker.rotQuaternion.set(buf);
-						//System.out.println("Rot: " + rotQuaternion.getX() + "," + rotQuaternion.getY() + "," + rotQuaternion.getZ() + "," + rotQuaternion.getW());
+						sensor.tracker.dataTick();
 						break;
 					case 2:
 						if(sensor == null)
@@ -192,7 +193,6 @@ public class TrackersUDPServer extends Thread {
 						bb.getLong();
 						stopCalibration(sensor);
 						sensor.tracker.gyroVector.set(bb.getFloat(), bb.getFloat(), bb.getFloat());
-						//System.out.println("Gyro: " + bb.getFloat() + "," + bb.getFloat() + "," + bb.getFloat());
 						break;
 					case 4:
 						if(sensor == null)
@@ -200,7 +200,6 @@ public class TrackersUDPServer extends Thread {
 						bb.getLong();
 						stopCalibration(sensor);
 						sensor.tracker.accelVector.set(bb.getFloat(), bb.getFloat(), bb.getFloat());
-						//System.out.println("Accel: " + bb.getFloat() + "," + bb.getFloat() + "," + bb.getFloat());
 						break;
 					case 5:
 						if(sensor == null)
@@ -208,7 +207,6 @@ public class TrackersUDPServer extends Thread {
 						bb.getLong();
 						stopCalibration(sensor);
 						sensor.tracker.magVector.set(bb.getFloat(), bb.getFloat(), bb.getFloat());
-						//System.out.println("Accel: " + bb.getFloat() + "," + bb.getFloat() + "," + bb.getFloat());
 						break;
 					case 6: // PACKET_RAW_CALIBRATION_DATA
 						if(sensor == null)
@@ -223,7 +221,7 @@ public class TrackersUDPServer extends Thread {
 						sensor.gyroCalibrationData = new double[] {bb.getFloat(), bb.getFloat(), bb.getFloat()};
 						break;
 					default:
-						System.out.println("[TrackerServer] Unknown data recieved: " + packetId + " from " + recieve.getSocketAddress());
+						System.out.println("[TrackerServer] Unknown data received: " + packetId + " from " + recieve.getSocketAddress());
 						break;
 					}
 					if(lastKeepup + 500 < System.currentTimeMillis()) {