Base for trackers server

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

@@ -0,0 +1,60 @@
+package io.eiren.vr;
+
+import java.util.List;
+
+import essentia.util.ann.ThreadSecure;
+import essentia.util.collections.FastList;
+import io.eiren.vr.bridge.NamedPipeVRBridge;
+import io.eiren.vr.processor.HumanPoseProcessor;
+import io.eiren.vr.trackers.HMDTracker;
+import io.eiren.vr.trackers.TrackersUDPServer;
+import io.eiren.vr.trackers.Tracker;
+
+public class VRServer extends Thread {
+	
+	private final List<Tracker> trackers = new FastList<>();
+	private final HumanPoseProcessor humanPoseProcessor;
+	private final TrackersUDPServer trackersServer = new TrackersUDPServer(6969, "Sensors UDP server", this::registerTracker);
+	private final NamedPipeVRBridge driverBridge;
+	
+	public VRServer() {
+		super("VRServer");
+		HMDTracker hmd = new HMDTracker("HMD");
+		humanPoseProcessor = new HumanPoseProcessor(hmd);
+		List<? extends Tracker> shareTrackers = humanPoseProcessor.getComputedTrackers();
+		driverBridge = new NamedPipeVRBridge(hmd, shareTrackers);
+		
+		registerTracker(hmd);
+		for(int i = 0; i < shareTrackers.size(); ++i)
+			registerTracker(shareTrackers.get(i));
+	}
+	
+	@Override
+	public void run() {
+		trackersServer.start();
+		driverBridge.start();
+		while(true) {
+			final long start = System.currentTimeMillis();
+			
+			humanPoseProcessor.update();
+			
+			final long time = System.currentTimeMillis() - start;
+			try {
+				Thread.sleep(Math.max(1, 10L - time)); // 100Hz
+			} catch(InterruptedException e) {
+			}
+		}
+	}
+	
+	private void autoAssignTracker(Tracker tracker) {
+		//
+	}
+	
+	@ThreadSecure
+	public void registerTracker(Tracker tracker) {
+		synchronized(trackers) {
+			trackers.add(tracker);
+		}
+		autoAssignTracker(tracker);
+	}
+}

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

@@ -4,29 +4,41 @@ import java.io.IOException;
 import java.nio.charset.Charset;
 import java.util.List;
 
+import com.jme3.math.Quaternion;
+import com.jme3.math.Vector3f;
 import com.sun.jna.platform.win32.Kernel32;
 import com.sun.jna.platform.win32.WinBase;
 import com.sun.jna.platform.win32.WinNT.HANDLE;
 import com.sun.jna.ptr.IntByReference;
 
 import essentia.util.collections.FastList;
-import io.eiren.util.StringUtils;
 import io.eiren.util.logging.LogManager;
+import io.eiren.vr.trackers.HMDTracker;
+import io.eiren.vr.trackers.Tracker;
+import io.eiren.vr.trackers.TrackerStatus;
 
 public class NamedPipeVRBridge extends Thread implements VRBridge {
 	
 	public static final String HMDPipeName = "\\\\.\\pipe\\HMDPipe";
 	public static final String TrackersPipeName = "\\\\.\\pipe\\TrackPipe";
+	public static final Charset ASCII = Charset.forName("ASCII");
 	
-	public static final int TRACKERS = 3;
-	private static final byte[] buffer = new byte[1024];
+	private final byte[] buffer = new byte[1024];
+	private final StringBuilder sbBuffer = new StringBuilder(1024);
+	private final Vector3f vBuffer = new Vector3f();
+	private final Quaternion qBuffer = new Quaternion();
 	
 	private Pipe hmdPipe;
-	private List<Pipe> trackerPipes = new FastList<>();
+	private final HMDTracker hmd;
+	private final List<Pipe> trackerPipes;
+	private final List<? extends Tracker> shareTrackers;
 	protected VRBridgeState bridgeState = VRBridgeState.NOT_STARTED;
 	
-	public NamedPipeVRBridge() {
+	public NamedPipeVRBridge(HMDTracker hmd, List<? extends Tracker> shareTrackers) {
 		super("Named Pipe VR Bridge");
+		this.hmd = hmd;
+		this.shareTrackers = new FastList<>(shareTrackers);
+		this.trackerPipes = new FastList<>(shareTrackers.size());
 	}
 	
 	@Override
@@ -58,9 +70,13 @@ public class NamedPipeVRBridge extends Thread implements VRBridge {
 						Thread.sleep(200L);
 					}
 				} else {
-					updateHMD();
-					for(int i = 0; i < trackerPipes.size(); ++i) {
-						updateTracker(trackerPipes.get(i), i);
+					// 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 {
+						Thread.sleep(5); // Up to 200Hz
 					}
 				}
 			}
@@ -70,12 +86,12 @@ public class NamedPipeVRBridge extends Thread implements VRBridge {
 		}
 	}
 	
-	public void updateHMD() {
+	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, Charset.forName("ASCII"));
+					String str = new String(buffer, 0, bytesAvailable.getValue() - 1, ASCII);
 					String[] split = str.split("\n")[0].split(" ");
 					try {
 						double x = Double.parseDouble(split[0]);
@@ -85,38 +101,45 @@ public class NamedPipeVRBridge extends Thread implements VRBridge {
 						double qx = Double.parseDouble(split[4]);
 						double qy = Double.parseDouble(split[5]);
 						double qz = Double.parseDouble(split[6]);
-						LogManager.log.info("[VRBridge] New HMD position:"
-								+ " " + StringUtils.prettyNumber((float) x, 2)
-								+ " " + StringUtils.prettyNumber((float) y, 2)
-								+ " " + StringUtils.prettyNumber((float) z, 2)
-								+ " " + StringUtils.prettyNumber((float) qw, 2)
-								+ " " + StringUtils.prettyNumber((float) qx, 2)
-								+ " " + StringUtils.prettyNumber((float) qy, 2)
-								+ " " + StringUtils.prettyNumber((float) qz, 2));
+						
+						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 false;
 	}
 	
-	public void updateTracker(Pipe pipe, int trackerId) {
-		
+	public void updateTracker(int trackerId) {
+		sbBuffer.setLength(0);
+		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);
 	}
 	
 	private void initHMDPipe(Pipe pipe) {
-		
+		hmd.setStatus(TrackerStatus.OK);
 	}
 	
 	private void initTrackerPipe(Pipe pipe, int trackerId) {
-		String trackerHello = TRACKERS + " 0";
-		byte[] buff = new byte[trackerHello.length() + 1];// = length of string + terminating '\0' !!!
-		System.arraycopy(trackerHello.getBytes(Charset.forName("ASCII")), 0, buff, 0, trackerHello.length());
+		String trackerHello = this.shareTrackers.size() + " 0";
+		System.arraycopy(trackerHello.getBytes(ASCII), 0, buffer, 0, trackerHello.length());
+		buffer[trackerHello.length()] = '\0';
 		IntByReference lpNumberOfBytesWritten = new IntByReference(0);
 		Kernel32.INSTANCE.WriteFile(pipe.pipeHandle,
-			buff,
-			buff.length,
+			buffer,
+			trackerHello.length() + 1,
 			lpNumberOfBytesWritten,
 			null);
 	}
@@ -153,7 +176,7 @@ public class NamedPipeVRBridge extends Thread implements VRBridge {
 			LogManager.log.info("[VRBridge] Pipe " + hmdPipe.name + " created");
 			if(WinBase.INVALID_HANDLE_VALUE.equals(hmdPipe.pipeHandle))
 				throw new IOException("Can't open " + HMDPipeName + " pipe: " + Kernel32.INSTANCE.GetLastError());
-			for(int i = 0; i < TRACKERS; ++i) {
+			for(int i = 0; i < this.shareTrackers.size(); ++i) {
 				String pipeName = TrackersPipeName + i;
 				HANDLE pipeHandle = Kernel32.INSTANCE.CreateNamedPipe(pipeName, WinBase.PIPE_ACCESS_DUPLEX, // dwOpenMode
 					WinBase.PIPE_TYPE_BYTE | WinBase.PIPE_READMODE_BYTE | WinBase.PIPE_WAIT, // dwPipeMode

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

@@ -0,0 +1,50 @@
+package io.eiren.vr.processor;
+
+import java.util.EnumMap;
+import java.util.List;
+
+import essentia.util.collections.FastList;
+import io.eiren.vr.trackers.ComputedTracker;
+import io.eiren.vr.trackers.HMDTracker;
+import io.eiren.vr.trackers.Tracker;
+
+public class HumanPoseProcessor {
+	
+	private final HMDTracker hmd;
+	private final List<ComputedTracker> computedTrackers = new FastList<>();
+	private final EnumMap<TrackerPosition, Tracker> trackers = new EnumMap<>(TrackerPosition.class);
+	private final ComputedTracker waist;
+	private final ComputedTracker leftFoot;
+	private final ComputedTracker rightFoot;
+	
+	public HumanPoseProcessor(HMDTracker hmd) {
+		this.hmd = hmd;
+		computedTrackers.add(waist = new ComputedTracker("Waist"));
+		computedTrackers.add(leftFoot = new ComputedTracker("Left Foot"));
+		computedTrackers.add(rightFoot = new ComputedTracker("Right Foot"));
+	}
+	
+	public List<? extends Tracker> getComputedTrackers() {
+		return computedTrackers;
+	}
+	
+	public void addTracker(Tracker tracker, TrackerPosition position) {
+		synchronized(trackers) {
+			trackers.put(position, tracker);
+		}
+	}
+	
+	public void update() {
+		
+	}
+	
+	public enum TrackerPosition {
+		NECK,
+		CHEST,
+		WAIST,
+		LEFT_LEG,
+		RIGHT_LEG,
+		LEFT_FOOT,
+		RIGHT_FOOT
+	}
+}

src/main/java/io/eiren/vr/sensors/RotationSensor.java

@@ -1,11 +0,0 @@
-package io.eiren.vr.sensors;
-
-import com.jme3.math.Quaternion;
-import com.jme3.math.Vector3f;
-
-public class RotationSensor {
-	public final Vector3f gyroVector = new Vector3f();
-	public final Vector3f accelVector = new Vector3f();
-	public final Vector3f magVector = new Vector3f();
-	public final Quaternion rotQuaternion = new Quaternion();
-}

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

@@ -0,0 +1,42 @@
+package io.eiren.vr.trackers;
+
+import com.jme3.math.Quaternion;
+import com.jme3.math.Vector3f;
+
+public class ComputedTracker implements Tracker {
+
+	public final Vector3f position = new Vector3f();
+	public final Quaternion rotation = new Quaternion();
+	protected final String name;
+	protected TrackerStatus status = TrackerStatus.DISCONNECTED;
+	
+	public ComputedTracker(String name) {
+		this.name = name;
+	}
+	
+	@Override
+	public String getName() {
+		return this.name;
+	}
+	
+	@Override
+	public boolean getPosition(Vector3f store) {
+		store.set(position);
+		return true;
+	}
+
+	@Override
+	public boolean getRotation(Quaternion store) {
+		store.set(rotation);
+		return true;
+	}
+
+	@Override
+	public TrackerStatus getStatus() {
+		return status;
+	}
+	
+	public void setStatus(TrackerStatus status) {
+		this.status = status;
+	}
+}

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

@@ -0,0 +1,9 @@
+package io.eiren.vr.trackers;
+
+public class HMDTracker extends ComputedTracker {
+
+	public HMDTracker(String name) {
+		super(name);
+	}
+	
+}

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

@@ -0,0 +1,51 @@
+package io.eiren.vr.trackers;
+
+import com.jme3.math.Quaternion;
+import com.jme3.math.Vector3f;
+
+public class IMUTracker implements Tracker {
+	
+	public final Vector3f gyroVector = new Vector3f();
+	public final Vector3f accelVector = new Vector3f();
+	public final Vector3f magVector = new Vector3f();
+	public final Quaternion rotQuaternion = new Quaternion();
+	protected TrackerStatus status = TrackerStatus.OK;
+	
+	protected final String name;
+	protected final TrackersUDPServer server;
+	
+	public IMUTracker(String name, TrackersUDPServer server) {
+		this.name = name;
+		this.server = server;
+	}
+	
+	@Override
+	public String getName() {
+		return this.name;
+	}
+	
+	@Override
+	public boolean getPosition(Vector3f store) {
+		store.set(0, 0, 0);
+		return false;
+	}
+	
+	@Override
+	public boolean getRotation(Quaternion store) {
+		store.set(rotQuaternion);
+		return true;
+	}
+
+	@Override
+	public TrackerStatus getStatus() {
+		return status;
+	}
+	
+	public void setStatus(TrackerStatus status) {
+		this.status = status;
+	}
+	
+	public void startCalibration() {
+		
+	}
+}

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

@@ -0,0 +1,15 @@
+package io.eiren.vr.trackers;
+
+import com.jme3.math.Quaternion;
+import com.jme3.math.Vector3f;
+
+public interface Tracker {
+	
+	public boolean getPosition(Vector3f store);
+	
+	public boolean getRotation(Quaternion store);
+	
+	public String getName();
+	
+	public TrackerStatus getStatus();
+}

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

@@ -0,0 +1,9 @@
+package io.eiren.vr.trackers;
+
+public enum TrackerStatus {
+	
+	DISCONNECTED,
+	OK,
+	BUSY,
+	ERROR;
+}

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

@@ -1,4 +1,4 @@
-package io.eiren.vr.sensors;
+package io.eiren.vr.trackers;
 
 import java.io.IOException;
 import java.io.UnsupportedEncodingException;
@@ -8,73 +8,83 @@ import java.net.SocketAddress;
 import java.net.SocketTimeoutException;
 import java.nio.ByteBuffer;
 import java.nio.ByteOrder;
-import java.nio.DoubleBuffer;
 import java.util.HashMap;
 import java.util.List;
 import java.util.Map;
-import java.util.function.Supplier;
+import java.util.function.Consumer;
 
-import org.apache.commons.math3.util.DoubleArray;
-
-import com.sun.jna.ptr.DoubleByReference;
+import com.jme3.math.Quaternion;
 
 import essentia.util.collections.FastList;
 import io.eiren.hardware.magentometer.Magneto;
 import io.eiren.util.Util;
 
-public class SensorUDPServer extends Thread {
+/**
+ * Recieves trackers data by UDP using extended owoTrack protocol.
+ */
+public class TrackersUDPServer extends Thread {
 	
 	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];
 
-	DatagramSocket socket = null;
-	byte[] sendBuffer = new byte[64];
-	long lastKeepup = System.currentTimeMillis();
-	private final Supplier<RotationSensor> sensorSupplier;
+	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<>();
+	private final Consumer<IMUTracker> trackersConsumer;
 	private final int port;
-	private List<SensorConnection> sensors = new FastList<>();
-	private Map<SocketAddress, SensorConnection> sensorsMap = new HashMap<>();
 	
-	public SensorUDPServer(int port, String name, Supplier<RotationSensor> sensorSupplier) {
+	protected DatagramSocket socket = null;
+	protected long lastKeepup = System.currentTimeMillis();
+	
+	public TrackersUDPServer(int port, String name, Consumer<IMUTracker> trackersConsumer) {
 		super(name);
 		this.port = port;
-		this.sensorSupplier = sensorSupplier;
+		this.trackersConsumer = trackersConsumer;
 	}
 	
-	public void sendCalibrationCommand(int sensorId) {
-		SensorConnection sensor;
-		synchronized(sensors) {
-			if(sensors.size() < sensorId + 1)
-				return;
-			sensor = sensors.get(sensorId);
+	public void sendCalibrationCommand(Tracker tracker) {
+		TrackerConnection connection = null;
+		synchronized(trackers) {
+			for(int i = 0; i < trackers.size(); ++i) {
+				if(trackers.get(i).tracker == tracker) {
+					connection = trackers.get(i);
+					break;
+				}
+			}
 		}
-		synchronized(sensor) {
-			if(sensor.isCalibrating)
+		if(connection == null)
+			return;
+		synchronized(connection) {
+			if(connection.isCalibrating)
 				return;
-			sensor.isCalibrating = true;
-			sensor.rawCalibrationData.clear();
+			connection.tracker.setStatus(TrackerStatus.BUSY);
+			connection.isCalibrating = true;
+			connection.rawCalibrationData.clear();
 		}
 		try {
-			socket.send(new DatagramPacket(CALIBRATION_BUFFER, CALIBRATION_BUFFER.length, sensor.address));
-			System.out.println("Calibrating sensor on " + sensor.address);
+			socket.send(new DatagramPacket(CALIBRATION_BUFFER, CALIBRATION_BUFFER.length, connection.address));
+			System.out.println("[TrackerServer] Calibrating sensor on " + connection.address);
 		} catch(IOException e) {
 			e.printStackTrace();
 		}
 	}
 	
-	private void stopCalibration(SensorConnection sensor) {
+	private void stopCalibration(TrackerConnection sensor) {
 		synchronized(sensor) {
 			if(!sensor.isCalibrating)
 				return;
 			if(sensor.gyroCalibrationData == null || sensor.rawCalibrationData.size() == 0)
 				return; // Calibration not started yet
+			sensor.tracker.setStatus(TrackerStatus.OK);
 			sensor.isCalibrating = false;
 		}
 		if(sensor.rawCalibrationData.size() > 50 && sensor.gyroCalibrationData != null) {
-			System.out.println("Gathered " + sensor.address + " calibrration data, processing...");
+			System.out.println("[TrackerServer] Gathered " + sensor.address + " calibrration data, processing...");
 		} else {
-			System.out.println("Can't gather enough calibration data, aboring...");
+			System.out.println("[TrackerServer] Can't gather enough calibration data, aboring...");
 			return;
 		}
 		double[] accelBasis = new double[3];
@@ -96,9 +106,9 @@ public class SensorUDPServer extends Thread {
 		}
 		
 		
-		System.out.println("Accelerometer Hnorm: " + Magneto.INSTANCE.calculateHnorm(accelData, sensor.rawCalibrationData.size()));
+		System.out.println("[TrackerServer] Accelerometer Hnorm: " + Magneto.INSTANCE.calculateHnorm(accelData, sensor.rawCalibrationData.size()));
 		Magneto.INSTANCE.calculate(accelData, sensor.rawCalibrationData.size(), 2, 10000, accelBasis, accelAInv);
-		System.out.println("Magentometer Hnorm: " + Magneto.INSTANCE.calculateHnorm(magData, sensor.rawCalibrationData.size()));
+		System.out.println("[TrackerServer] Magentometer Hnorm: " + Magneto.INSTANCE.calculateHnorm(magData, sensor.rawCalibrationData.size()));
 		Magneto.INSTANCE.calculate(magData, sensor.rawCalibrationData.size(), 2, 100, magBasis, magAInv);
 		
 		System.out.println("float G_off[3] =");
@@ -118,21 +128,23 @@ public class SensorUDPServer extends Thread {
 	}
 	
 	private void setUpNewSensor(DatagramPacket handshakePacket, ByteBuffer data) throws IOException {
-		System.out.println("Handshake recieved from " + handshakePacket.getAddress() + ":" + handshakePacket.getPort());
+		System.out.println("[TrackerServer] Handshake recieved from " + handshakePacket.getAddress() + ":" + handshakePacket.getPort());
 		SocketAddress addr = handshakePacket.getSocketAddress();
-		SensorConnection sensor;
-		synchronized(sensors) {
-			sensor = sensorsMap.get(addr);
+		TrackerConnection sensor;
+		synchronized(trackers) {
+			sensor = trackersMap.get(addr);
 		}
 		if(sensor == null) {
-			sensor = new SensorConnection(sensorSupplier.get(), addr);
+			IMUTracker imu = new IMUTracker(handshakePacket.getSocketAddress().toString(), this);
+			trackersConsumer.accept(imu);
+			sensor = new TrackerConnection(imu, addr);
 			int i = 0;
-			synchronized(sensors) {
-				i = sensors.size();
-				sensors.add(sensor);
-				sensorsMap.put(addr, sensor);
+			synchronized(trackers) {
+				i = trackers.size();
+				trackers.add(sensor);
+				trackersMap.put(addr, sensor);
 			}
-			System.out.println("Sensor " + i + " added with address " + addr);
+			System.out.println("[TrackerServer] Sensor " + i + " added with address " + addr);
 		}
         socket.send(new DatagramPacket(HANDSHAKE_BUFFER, HANDSHAKE_BUFFER.length, handshakePacket.getAddress(), handshakePacket.getPort()));
 	}
@@ -149,9 +161,9 @@ public class SensorUDPServer extends Thread {
 					socket.receive(recieve);
 					bb.rewind();
 
-					SensorConnection sensor;
-					synchronized(sensors) {
-						sensor = sensorsMap.get(recieve.getSocketAddress());
+					TrackerConnection sensor;
+					synchronized(trackers) {
+						sensor = trackersMap.get(recieve.getSocketAddress());
 					}
 					int packetId;
 					switch(packetId = bb.getInt()) {
@@ -163,8 +175,9 @@ public class SensorUDPServer extends Thread {
 							break;
 						bb.getLong();
 						stopCalibration(sensor);
-						sensor.sensor.rotQuaternion.set(bb.getFloat(), bb.getFloat(), bb.getFloat(), bb.getFloat());
-						//rotQuaternion.set(-rotQuaternion.getY(), rotQuaternion.getX(), rotQuaternion.getZ(), rotQuaternion.getW());
+						buf.set(bb.getFloat(), bb.getFloat(), bb.getFloat(), bb.getFloat());
+						buf.set(-buf.getX(), buf.getZ(), buf.getY(), buf.getW()); // Change from sensor rotation to OpenGL/SteamVR rotation
+						sensor.tracker.rotQuaternion.set(buf);
 						//System.out.println("Rot: " + rotQuaternion.getX() + "," + rotQuaternion.getY() + "," + rotQuaternion.getZ() + "," + rotQuaternion.getW());
 						break;
 					case 2:
@@ -172,7 +185,7 @@ public class SensorUDPServer extends Thread {
 							break;
 						bb.getLong();
 						stopCalibration(sensor);
-						sensor.sensor.gyroVector.set(bb.getFloat(), bb.getFloat(), bb.getFloat());
+						sensor.tracker.gyroVector.set(bb.getFloat(), bb.getFloat(), bb.getFloat());
 						//System.out.println("Gyro: " + bb.getFloat() + "," + bb.getFloat() + "," + bb.getFloat());
 						break;
 					case 4:
@@ -180,7 +193,7 @@ public class SensorUDPServer extends Thread {
 							break;
 						bb.getLong();
 						stopCalibration(sensor);
-						sensor.sensor.accelVector.set(bb.getFloat(), bb.getFloat(), bb.getFloat());
+						sensor.tracker.accelVector.set(bb.getFloat(), bb.getFloat(), bb.getFloat());
 						//System.out.println("Accel: " + bb.getFloat() + "," + bb.getFloat() + "," + bb.getFloat());
 						break;
 					case 5:
@@ -188,7 +201,7 @@ public class SensorUDPServer extends Thread {
 							break;
 						bb.getLong();
 						stopCalibration(sensor);
-						sensor.sensor.magVector.set(bb.getFloat(), bb.getFloat(), bb.getFloat());
+						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
@@ -204,14 +217,14 @@ public class SensorUDPServer extends Thread {
 						sensor.gyroCalibrationData = new double[] {bb.getFloat(), bb.getFloat(), bb.getFloat()};
 						break;
 					default:
-						System.out.println("Unknown data recieved: " + packetId);
+						System.out.println("[TrackerServer] Unknown data recieved: " + packetId + " from " + recieve.getSocketAddress());
 						break;
 					}
 					if(lastKeepup + 500 < System.currentTimeMillis()) {
 						lastKeepup = System.currentTimeMillis();
-						synchronized(sensors) {
-							for(int i = 0; i < sensors.size(); ++i)
-								socket.send(new DatagramPacket(KEEPUP_BUFFER, KEEPUP_BUFFER.length, sensors.get(i).address));
+						synchronized(trackers) {
+							for(int i = 0; i < trackers.size(); ++i)
+								socket.send(new DatagramPacket(KEEPUP_BUFFER, KEEPUP_BUFFER.length, trackers.get(i).address));
 						}
 					}
 				} catch(SocketTimeoutException e) {
@@ -226,16 +239,16 @@ public class SensorUDPServer extends Thread {
 		}
 	}
 	
-	private class SensorConnection {
+	private class TrackerConnection {
 		
-		RotationSensor sensor;
+		IMUTracker tracker;
 		SocketAddress address;
 		boolean isCalibrating;
 		private List<double[]> rawCalibrationData = new FastList<>();
 		private double[] gyroCalibrationData;
 		
-		public SensorConnection(RotationSensor sensor, SocketAddress address) {
-			this.sensor = sensor;
+		public TrackerConnection(IMUTracker tracker, SocketAddress address) {
+			this.tracker = tracker;
 			this.address = address;
 		}
 	}