Fixed typo

Fix some typos

src/main/java/io/eiren/gui/SkeletonConfig.java

@@ -1,16 +1,20 @@
 package io.eiren.gui;
 
+import java.awt.event.ItemEvent;
+import java.awt.event.ItemListener;
 import java.awt.event.MouseEvent;
 import java.util.HashMap;
 import java.util.Map;
 
 import javax.swing.JButton;
+import javax.swing.JCheckBox;
 import javax.swing.JLabel;
 import javax.swing.event.MouseInputAdapter;
 
 import io.eiren.util.StringUtils;
 import io.eiren.util.ann.ThreadSafe;
 import io.eiren.vr.VRServer;
+import io.eiren.vr.processor.HumanSkeletonWithLegs;
 import io.eiren.vr.processor.HumanSkeleton;
 
 public class SkeletonConfig extends EJBag {
@@ -26,6 +30,7 @@ public class SkeletonConfig extends EJBag {
 
 		setAlignmentY(TOP_ALIGNMENT);
 		server.humanPoseProcessor.addSkeletonUpdatedCallback(this::skeletonUpdated);
+		skeletonUpdated(null);
 	}
 	
 	@ThreadSafe
@@ -34,6 +39,56 @@ public class SkeletonConfig extends EJBag {
 			removeAll();
 			
 			int row = 0;
+
+			add(new JCheckBox("Extended pelvis model") {{
+				addItemListener(new ItemListener() {
+				    @Override
+				    public void itemStateChanged(ItemEvent e) {
+				        if(e.getStateChange() == ItemEvent.SELECTED) {//checkbox has been selected
+				        	if(newSkeleton != null && newSkeleton instanceof HumanSkeletonWithLegs) {
+				        		HumanSkeletonWithLegs hswl = (HumanSkeletonWithLegs) newSkeleton;
+				        		hswl.setSkeletonConfigBoolean("Extended pelvis model", true);
+				        	}
+				        } else {
+				        	if(newSkeleton != null && newSkeleton instanceof HumanSkeletonWithLegs) {
+				        		HumanSkeletonWithLegs hswl = (HumanSkeletonWithLegs) newSkeleton;
+				        		hswl.setSkeletonConfigBoolean("Extended pelvis model", false);
+				        	}
+				        }
+				    }
+				});
+				if(newSkeleton != null && newSkeleton instanceof HumanSkeletonWithLegs) {
+	        		HumanSkeletonWithLegs hswl = (HumanSkeletonWithLegs) newSkeleton;
+	        		setSelected(hswl.getSkeletonConfigBoolean("Extended pelvis model"));
+				}
+			}}, s(c(0, row, 1), 3, 1));
+			row++;
+			
+			/*
+			add(new JCheckBox("Extended knee model") {{
+				addItemListener(new ItemListener() {
+				    @Override
+				    public void itemStateChanged(ItemEvent e) {
+				        if(e.getStateChange() == ItemEvent.SELECTED) {//checkbox has been selected
+				        	if(newSkeleton != null && newSkeleton instanceof HumanSkeletonWithLegs) {
+				        		HumanSkeletonWithLegs hswl = (HumanSkeletonWithLegs) newSkeleton;
+				        		hswl.setSkeletonConfigBoolean("Extended knee model", true);
+				        	}
+				        } else {
+				        	if(newSkeleton != null && newSkeleton instanceof HumanSkeletonWithLegs) {
+				        		HumanSkeletonWithLegs hswl = (HumanSkeletonWithLegs) newSkeleton;
+				        		hswl.setSkeletonConfigBoolean("Extended knee model", false);
+				        	}
+				        }
+				    }
+				});
+				if(newSkeleton != null && newSkeleton instanceof HumanSkeletonWithLegs) {
+	        		HumanSkeletonWithLegs hswl = (HumanSkeletonWithLegs) newSkeleton;
+	        		setSelected(hswl.getSkeletonConfigBoolean("Extended knee model"));
+				}
+			}}, s(c(0, row, 1), 3, 1));
+			row++;
+			//*/
 			
 			add(new TimedResetButton("Reset All", "All"), s(c(1, row, 1), 3, 1));
 			row++;

src/main/java/io/eiren/gui/TrackersList.java

@@ -102,8 +102,12 @@ public class TrackersList extends EJBox {
 		JLabel bat;
 		JLabel ping;
 		JLabel raw;
+		JLabel rawMag;
+		JLabel calibration;
+		JLabel magAccuracy;
 		JLabel adj;
 		JLabel adjYaw;
+		JLabel correction;
 		
 		@AWTThread
 		public TrackerRow(Tracker t) {
@@ -114,12 +118,19 @@ public class TrackersList extends EJBox {
 		@SuppressWarnings("unchecked")
 		@AWTThread
 		public TrackerRow build() {
+			int row = 0;
+			
+			Tracker realTracker = t;
+			if(t instanceof ReferenceAdjustedTracker)
+				realTracker = ((ReferenceAdjustedTracker<? extends Tracker>) t).getTracker();
 			removeAll();
-			add(new JLabel(t.getName()), s(c(0, 0, 0, GridBagConstraints.FIRST_LINE_START), 4, 1));
+			add(new JLabel(t.getName()), s(c(0, row, 0, GridBagConstraints.FIRST_LINE_START), 4, 1));
+			row++;
+			
 			if(t.userEditable()) {
 				TrackerConfig cfg = server.getTrackerConfig(t);
 				JComboBox<String> desSelect;
-				add(desSelect = new JComboBox<>(), s(c(0, 1, 0, GridBagConstraints.FIRST_LINE_START), 2, 1));
+				add(desSelect = new JComboBox<>(), s(c(0, row, 0, GridBagConstraints.FIRST_LINE_START), 2, 1));
 				for(TrackerBodyPosition p : TrackerBodyPosition.values) {
 					desSelect.addItem(p.name());
 				}
@@ -136,14 +147,11 @@ public class TrackersList extends EJBox {
 						server.trackerUpdated(t);
 					}
 				});
-				Tracker realTracker = t;
-				if(t instanceof ReferenceAdjustedTracker<?>)
-					realTracker = ((ReferenceAdjustedTracker<? extends Tracker>) t).getTracker();
 				if(realTracker instanceof IMUTracker) {
 					IMUTracker imu = (IMUTracker) realTracker;
 					TrackerMountingRotation tr = imu.getMountingRotation();
 					JComboBox<String> mountSelect;
-					add(mountSelect = new JComboBox<>(), s(c(2, 1, 0, GridBagConstraints.FIRST_LINE_START), 2, 1));
+					add(mountSelect = new JComboBox<>(), s(c(2, row, 0, GridBagConstraints.FIRST_LINE_START), 2, 1));
 					for(TrackerMountingRotation p : TrackerMountingRotation.values) {
 						mountSelect.addItem(p.name());
 					}
@@ -161,31 +169,49 @@ public class TrackersList extends EJBox {
 						}
 					});
 				}
+				row++;
 			}
-			add(new JLabel("Rotation"), c(0, 2, 0, GridBagConstraints.FIRST_LINE_START));
-			add(new JLabel("Postion"), c(1, 2, 0, GridBagConstraints.FIRST_LINE_START));
-			add(new JLabel("Ping"), c(2, 2, 0, GridBagConstraints.FIRST_LINE_START));
-			add(new JLabel("TPS"), c(3, 2, 0, GridBagConstraints.FIRST_LINE_START));
-			add(rotation = new JLabel("0 0 0"), c(0, 3, 0, GridBagConstraints.FIRST_LINE_START));
-			add(position = new JLabel("0 0 0"), c(1, 3, 0, GridBagConstraints.FIRST_LINE_START));
-			add(ping = new JLabel(""), c(2, 3, 0, GridBagConstraints.FIRST_LINE_START));
-			if(t instanceof TrackerWithTPS) {
-				add(tps = new JLabel("0"), c(3, 3, 0, GridBagConstraints.FIRST_LINE_START));
+			add(new JLabel("Rotation"), c(0, row, 0, GridBagConstraints.FIRST_LINE_START));
+			add(new JLabel("Position"), c(1, row, 0, GridBagConstraints.FIRST_LINE_START));
+			add(new JLabel("Ping"), c(2, row, 0, GridBagConstraints.FIRST_LINE_START));
+			add(new JLabel("TPS"), c(3, row, 0, GridBagConstraints.FIRST_LINE_START));
+			row++;
+			add(rotation = new JLabel("0 0 0"), c(0, row, 0, GridBagConstraints.FIRST_LINE_START));
+			add(position = new JLabel("0 0 0"), c(1, row, 0, GridBagConstraints.FIRST_LINE_START));
+			add(ping = new JLabel(""), c(2, row, 0, GridBagConstraints.FIRST_LINE_START));
+			if(realTracker instanceof TrackerWithTPS) {
+				add(tps = new JLabel("0"), c(3, row, 0, GridBagConstraints.FIRST_LINE_START));
 			} else {
-				add(new JLabel(""), c(3, 3, 0, GridBagConstraints.FIRST_LINE_START));
+				add(new JLabel(""), c(3, row, 0, GridBagConstraints.FIRST_LINE_START));
+			}
+			row++;
+			add(new JLabel("Status:"), c(0, row, 0, GridBagConstraints.FIRST_LINE_START));
+			add(status = new JLabel(t.getStatus().toString().toLowerCase()), c(1, row, 0, GridBagConstraints.FIRST_LINE_START));
+			add(new JLabel("Battery:"), c(2, row, 0, GridBagConstraints.FIRST_LINE_START));
+			add(bat = new JLabel("0"), c(3, row, 0, GridBagConstraints.FIRST_LINE_START));
+			row++;
+			add(new JLabel("Raw:"), c(0, row, 0, GridBagConstraints.FIRST_LINE_START));
+			add(raw = new JLabel("0 0 0"), s(c(1, row, 0, GridBagConstraints.FIRST_LINE_START), 3, 1));
+			row++;
+			if(realTracker instanceof IMUTracker) {
+				add(new JLabel("Raw mag:"), c(0, row, 0, GridBagConstraints.FIRST_LINE_START));
+				add(rawMag = new JLabel("0 0 0"), s(c(1, row, 0, GridBagConstraints.FIRST_LINE_START), 3, 1));
+				row++;
+				add(new JLabel("Cal:"), c(0, row, 0, GridBagConstraints.FIRST_LINE_START));
+				add(calibration = new JLabel("0"), c(1, row, 0, GridBagConstraints.FIRST_LINE_START));
+				add(new JLabel("Mag acc:"), c(2, row, 0, GridBagConstraints.FIRST_LINE_START));
+				add(magAccuracy = new JLabel("0°"), c(3, row, 0, GridBagConstraints.FIRST_LINE_START));
+				row++;
+				add(new JLabel("Correction:"), c(0, row, 0, GridBagConstraints.FIRST_LINE_START));
+				add(correction = new JLabel("0 0 0"), s(c(1, row, 0, GridBagConstraints.FIRST_LINE_START), 3, 1));
+				row++;
 			}
-			add(new JLabel("Status:"), c(0, 4, 0, GridBagConstraints.FIRST_LINE_START));
-			add(status = new JLabel(t.getStatus().toString().toLowerCase()), c(1, 4, 0, GridBagConstraints.FIRST_LINE_START));
-			add(new JLabel("Battery:"), c(2, 4, 0, GridBagConstraints.FIRST_LINE_START));
-			add(bat = new JLabel("0"), c(3, 4, 0, GridBagConstraints.FIRST_LINE_START));
-			add(new JLabel("Raw:"), c(0, 5, 0, GridBagConstraints.FIRST_LINE_START));
-			add(raw = new JLabel("0 0 0"), s(c(1, 5, 0, GridBagConstraints.FIRST_LINE_START), 3, 1));
 			
-			if(t instanceof ReferenceAdjustedTracker) {
-				add(new JLabel("Adj:"), c(0, 6, 0, GridBagConstraints.FIRST_LINE_START));
-				add(adj = new JLabel("0 0 0 0"), c(1, 6, 0, GridBagConstraints.FIRST_LINE_START));
-				add(new JLabel("AdjY:"), c(2, 6, 0, GridBagConstraints.FIRST_LINE_START));
-				add(adjYaw = new JLabel("0 0 0 0"), c(3, 6, 0, GridBagConstraints.FIRST_LINE_START));
+			if(t instanceof ReferenceAdjustedTracker) {	
+				add(new JLabel("Adj:"), c(0, row, 0, GridBagConstraints.FIRST_LINE_START));
+				add(adj = new JLabel("0 0 0 0"), c(1, row, 0, GridBagConstraints.FIRST_LINE_START));
+				add(new JLabel("AdjY:"), c(2, row, 0, GridBagConstraints.FIRST_LINE_START));
+				add(adjYaw = new JLabel("0 0 0 0"), c(3, row, 0, GridBagConstraints.FIRST_LINE_START));
 			}
 
 			setBorder(BorderFactory.createLineBorder(new Color(0x663399), 4, true));
@@ -198,6 +224,9 @@ public class TrackersList extends EJBox {
 		public void update() {
 			if(position == null)
 				return;
+			Tracker realTracker = t;
+			if(t instanceof ReferenceAdjustedTracker)
+				realTracker = ((ReferenceAdjustedTracker<? extends Tracker>) t).getTracker();
 			t.getRotation(q);
 			t.getPosition(v);
 			q.toAngles(angles);
@@ -210,14 +239,12 @@ public class TrackersList extends EJBox {
 					+ " " + StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0));
 			status.setText(t.getStatus().toString().toLowerCase());
 			
-			if(t instanceof TrackerWithTPS) {
-				tps.setText(StringUtils.prettyNumber(((TrackerWithTPS) t).getTPS(), 1));
+			if(realTracker instanceof TrackerWithTPS) {
+				tps.setText(StringUtils.prettyNumber(((TrackerWithTPS) realTracker).getTPS(), 1));
 			}
-			if(t instanceof TrackerWithBattery)
-				bat.setText(StringUtils.prettyNumber(((TrackerWithBattery) t).getBatteryVoltage(), 1));
-			Tracker t2 = t;
+			if(realTracker instanceof TrackerWithBattery)
+				bat.setText(StringUtils.prettyNumber(((TrackerWithBattery) realTracker).getBatteryVoltage(), 1));
 			if(t instanceof ReferenceAdjustedTracker) {
-				t2 = ((ReferenceAdjustedTracker<Tracker>) t).getTracker();
 				((ReferenceAdjustedTracker<Tracker>) t).attachmentFix.toAngles(angles);
 				adj.setText(StringUtils.prettyNumber(angles[0] * FastMath.RAD_TO_DEG, 0)
 						+ " " + StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 0)
@@ -227,13 +254,27 @@ public class TrackersList extends EJBox {
 						+ " " + StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 0)
 						+ " " + StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0));
 			}
-			if(t2 instanceof IMUTracker)
-				ping.setText(String.valueOf(((IMUTracker) t2).ping));
-			t2.getRotation(q);
+			if(realTracker instanceof IMUTracker) {
+				ping.setText(String.valueOf(((IMUTracker) realTracker).ping));
+			}
+			realTracker.getRotation(q);
 			q.toAngles(angles);
 			raw.setText(StringUtils.prettyNumber(angles[0] * FastMath.RAD_TO_DEG, 0)
 					+ " " + StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 0)
 					+ " " + StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0));
+			if(realTracker instanceof IMUTracker) {
+				((IMUTracker) realTracker).rotMagQuaternion.toAngles(angles);
+				rawMag.setText(StringUtils.prettyNumber(angles[0] * FastMath.RAD_TO_DEG, 0)
+						+ " " + StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 0)
+						+ " " + StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0));
+				calibration.setText(((IMUTracker) realTracker).calibrationStatus + " / " + ((IMUTracker) realTracker).magCalibrationStatus);
+				magAccuracy.setText(StringUtils.prettyNumber(((IMUTracker) realTracker).magnetometerAccuracy * FastMath.RAD_TO_DEG, 1) + "°");
+				((IMUTracker) realTracker).getCorrection(q);
+				q.toAngles(angles);
+				correction.setText(StringUtils.prettyNumber(angles[0] * FastMath.RAD_TO_DEG, 0)
+						+ " " + StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 0)
+						+ " " + StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0));
+			}
 		}
 	}
 	

src/main/java/io/eiren/gui/VRServerGUI.java

@@ -91,6 +91,15 @@ public class VRServerGUI extends JFrame {
 					}
 				});
 			}});
+			add(Box.createHorizontalStrut(10));
+			add(new JButton("Fast Reset") {{
+				addMouseListener(new MouseInputAdapter() {
+					@Override
+					public void mouseClicked(MouseEvent e) {
+						resetFast();
+					}
+				});
+			}});
 			add(Box.createHorizontalGlue());
 			add(new JButton("GUI Zoom (x" + StringUtils.prettyNumber(zoom, 2) + ")") {{
 				addMouseListener(new MouseInputAdapter() {
@@ -126,7 +135,7 @@ public class VRServerGUI extends JFrame {
 				trackersSelect.addItem("Waist + Feet + Chest");
 				trackersSelect.addItem("Waist + Feet + Knees");
 				trackersSelect.addItem("Waist + Feet + Chest + Knees");
-				switch(server.config.getInt("vitrualtrackers", 3)) {
+				switch(server.config.getInt("virtualtrackers", 3)) {
 				case 1:
 					trackersSelect.setSelectedIndex(0);
 					break;
@@ -148,19 +157,19 @@ public class VRServerGUI extends JFrame {
 					public void actionPerformed(ActionEvent e) {
 						switch(trackersSelect.getSelectedIndex()) {
 						case 0:
-							server.config.setProperty("vitrualtrackers", 1);
+							server.config.setProperty("virtualtrackers", 1);
 							break;
 						case 1:
-							server.config.setProperty("vitrualtrackers", 3);
+							server.config.setProperty("virtualtrackers", 3);
 							break;
 						case 2:
-							server.config.setProperty("vitrualtrackers", 4);
+							server.config.setProperty("virtualtrackers", 4);
 							break;
 						case 3:
-							server.config.setProperty("vitrualtrackers", 5);
+							server.config.setProperty("virtualtrackers", 5);
 							break;
 						case 4:
-							server.config.setProperty("vitrualtrackers", 6);
+							server.config.setProperty("virtualtrackers", 6);
 							break;
 						}
 						server.saveConfig();
@@ -234,6 +243,11 @@ public class VRServerGUI extends JFrame {
 		}
 	}
 	
+	@AWTThread
+	private void resetFast() {
+		server.resetTrackersYaw();
+	}
+	
 	@AWTThread
 	private void reset() {
 		ButtonTimer.runTimer(resetButton, 3, "RESET", server::resetTrackers);

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

@@ -7,7 +7,7 @@ import io.eiren.util.logging.LogManager;
 
 public class Main {
 	
-	public static String VERSION = "0.0.13";
+	public static String VERSION = "0.0.14";
 	
 	public static VRServer vrServer;
 	

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

@@ -50,7 +50,7 @@ public class VRServer extends Thread {
 		hmdTracker = new HMDTracker("HMD");
 		hmdTracker.position.set(0, 1.8f, 0); // Set starting position for easier debugging
 		// TODO Multiple processors
-		humanPoseProcessor = new HumanPoseProcessor(this, hmdTracker, config.getInt("vitrualtrackers", 3));
+		humanPoseProcessor = new HumanPoseProcessor(this, hmdTracker, config.getInt("virtualtrackers", 3));
 		List<? extends Tracker> shareTrackers = humanPoseProcessor.getComputedTrackers();
 		
 		// Create named pipe bridge for SteamVR driver
@@ -189,6 +189,8 @@ public class VRServer extends Thread {
 					break;
 				task.run();
 			} while(true);
+			for(int i = 0; i < trackers.size(); ++i)
+				trackers.get(i).tick();
 			
 			for(int i = 0; i < onTick.size(); ++i) {
 				this.onTick.get(i).run();
@@ -234,6 +236,12 @@ public class VRServer extends Thread {
 		});
 	}
 	
+	public void resetTrackersYaw() {
+		queueTask(() -> {
+			humanPoseProcessor.resetTrackersYaw();
+		});
+	}
+	
 	public int getTrackersCount() {
 		return trackers.size();
 	}

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

@@ -34,6 +34,10 @@ public class HumanPoseProcessor {
 			}
 		}
 	}
+	
+	public HumanSkeleton getSkeleton() {
+		return skeleton;
+	}
 
 	@VRServerThread
 	public void addSkeletonUpdatedCallback(Consumer<HumanSkeleton> consumer) {
@@ -85,24 +89,22 @@ public class HumanPoseProcessor {
 		boolean hasBothLegs = false;
 		List<Tracker> allTrackers = server.getAllTrackers();
 		Tracker waist = TrackerUtils.findTrackerForBodyPosition(allTrackers, TrackerBodyPosition.WAIST, TrackerBodyPosition.CHEST);
-		Tracker leftAnkle = TrackerUtils.findTrackerForBodyPosition(allTrackers, TrackerBodyPosition.LEFT_ANKLE);
-		Tracker rightAnkle = TrackerUtils.findTrackerForBodyPosition(allTrackers, TrackerBodyPosition.RIGHT_ANKLE);
-		Tracker leftLeg = TrackerUtils.findTrackerForBodyPosition(allTrackers, TrackerBodyPosition.LEFT_LEG);
-		Tracker rightLeg = TrackerUtils.findTrackerForBodyPosition(allTrackers, TrackerBodyPosition.RIGHT_LEG);
+		Tracker leftAnkle = TrackerUtils.findTrackerForBodyPosition(allTrackers, TrackerBodyPosition.LEFT_ANKLE, TrackerBodyPosition.LEFT_LEG);
+		Tracker rightAnkle = TrackerUtils.findTrackerForBodyPosition(allTrackers, TrackerBodyPosition.RIGHT_ANKLE, TrackerBodyPosition.RIGHT_LEG);
 		if(waist != null)
 			hasWaist = true;
-		if(leftAnkle != null && rightAnkle != null && leftLeg != null && rightLeg != null)
+		if(leftAnkle != null && rightAnkle != null)
 			hasBothLegs = true;
 		if(!hasWaist) {
 			skeleton = null; // Can't track anything without waist
 		} else if(hasBothLegs) {
 			disconnectAllTrackers();
-			skeleton = new HumanSekeletonWithLegs(server, computedTrackers);
+			skeleton = new HumanSkeletonWithLegs(server, computedTrackers);
 			for(int i = 0; i < onSkeletonUpdated.size(); ++i)
 				onSkeletonUpdated.get(i).accept(skeleton);
 		} else {
 			disconnectAllTrackers();
-			skeleton = new HumanSkeleonWithWaist(server, computedTrackers);
+			skeleton = new HumanSkeletonWithWaist(server, computedTrackers);
 			for(int i = 0; i < onSkeletonUpdated.size(); ++i)
 				onSkeletonUpdated.get(i).accept(skeleton);
 		}
@@ -126,4 +128,10 @@ public class HumanPoseProcessor {
 		if(skeleton != null)
 			skeleton.resetTrackersFull();
 	}
+
+	@VRServerThread
+	public void resetTrackersYaw() {
+		if(skeleton != null)
+			skeleton.resetTrackersYaw();
+	}
 }

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

@@ -24,4 +24,7 @@ public abstract class HumanSkeleton {
 
 	@VRServerThread
 	public abstract void resetTrackersFull();
+	
+	@VRServerThread
+	public abstract void resetTrackersYaw();
 }

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

@@ -12,16 +12,17 @@ import io.eiren.vr.trackers.Tracker;
 import io.eiren.vr.trackers.TrackerStatus;
 import io.eiren.vr.trackers.TrackerUtils;
 
-public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
+public class HumanSkeletonWithLegs extends HumanSkeletonWithWaist {
 	
 	public static final float HIPS_WIDTH_DEFAULT = 0.3f;
 	public static final float FOOT_LENGTH_DEFAULT = 0.05f;
 	public static final float DEFAULT_FLOOR_OFFSET = 0.05f;
 	
-	protected final float[] kneeAngles = new float[3];
-	protected final float[] hipAngles = new float[3];
 	protected final Quaternion hipBuf = new Quaternion();
 	protected final Quaternion kneeBuf = new Quaternion();
+	protected final Vector3f hipVector = new Vector3f();
+	protected final Vector3f ankleVector = new Vector3f();
+	protected final Quaternion kneeRotation = new Quaternion();
 	
 	protected final Tracker leftLegTracker;
 	protected final Tracker leftAnkleTracker;
@@ -61,15 +62,18 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 	protected float maxKneePitch = 90f * FastMath.DEG_TO_RAD;
 	
 	protected float kneeLerpFactor = 0.5f;
+	
+	protected boolean extendedPelvisModel = true;
+	protected boolean extendedKneeModel = false;
 
-	public HumanSekeletonWithLegs(VRServer server, List<ComputedHumanPoseTracker> computedTrackers) {
+	public HumanSkeletonWithLegs(VRServer server, List<ComputedHumanPoseTracker> computedTrackers) {
 		super(server, computedTrackers);
 		List<Tracker> allTracekrs = server.getAllTrackers();
-		this.leftLegTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.LEFT_LEG);
-		this.leftAnkleTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.LEFT_ANKLE);
+		this.leftLegTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.LEFT_LEG, TrackerBodyPosition.LEFT_ANKLE);
+		this.leftAnkleTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.LEFT_ANKLE, TrackerBodyPosition.LEFT_LEG);
 		this.leftFootTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.LEFT_FOOT);
-		this.rightLegTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.RIGHT_LEG);
-		this.rightAnkleTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.RIGHT_ANKLE);
+		this.rightLegTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.RIGHT_LEG, TrackerBodyPosition.RIGHT_ANKLE);
+		this.rightAnkleTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.RIGHT_ANKLE, TrackerBodyPosition.RIGHT_LEG);
 		this.rightFootTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.RIGHT_FOOT);
 		ComputedHumanPoseTracker lat = null;
 		ComputedHumanPoseTracker rat = null;
@@ -96,6 +100,8 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 		kneeHeight = server.config.getFloat("body.kneeHeight", kneeHeight);
 		legsLength = server.config.getFloat("body.legsLength", legsLength);
 		footLength = server.config.getFloat("body.footLength", footLength);
+		extendedPelvisModel = server.config.getBoolean("body.model.extendedPelvis", extendedPelvisModel);
+		extendedKneeModel = server.config.getBoolean("body.model.extendedKnee", extendedKneeModel);
 		
 		waistNode.attachChild(leftHipNode);
 		leftHipNode.localTransform.setTranslation(-hipsWidth / 2, 0, 0);
@@ -191,6 +197,28 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 		}
 	}
 	
+	public boolean getSkeletonConfigBoolean(String config) {
+		switch(config) {
+		case "Extended pelvis model":
+			return extendedPelvisModel;
+		case "Extended knee model":
+			return extendedKneeModel;
+		}
+		return false;
+	}
+	
+	public void setSkeletonConfigBoolean(String config, boolean newState) {
+		switch(config) {
+		case "Extended pelvis model":
+			extendedPelvisModel = newState;
+			server.config.setProperty("body.model.extendedPelvis", newState);
+			break;
+		case "Extended knee model":
+			extendedKneeModel = newState;
+			server.config.setProperty("body.model.extendedKnee", newState);
+		}
+	}
+	
 	@Override
 	public void updateLocalTransforms() {
 		super.updateLocalTransforms();
@@ -198,7 +226,8 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 		leftLegTracker.getRotation(hipBuf);
 		leftAnkleTracker.getRotation(kneeBuf);
 
-		//calculateKneeLimits(hipBuf, kneeBuf, leftLegTracker.getConfidenceLevel(), leftAnkleTracker.getConfidenceLevel());
+		if(extendedKneeModel)
+			calculateKneeLimits(hipBuf, kneeBuf, leftLegTracker.getConfidenceLevel(), leftAnkleTracker.getConfidenceLevel());
 		
 		leftHipNode.localTransform.setRotation(hipBuf);
 		leftKneeNode.localTransform.setRotation(kneeBuf);
@@ -215,7 +244,8 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 		rightLegTracker.getRotation(hipBuf);
 		rightAnkleTracker.getRotation(kneeBuf);
 		
-		//calculateKneeLimits(hipBuf, kneeBuf, rightLegTracker.getConfidenceLevel(), rightAnkleTracker.getConfidenceLevel());
+		if(extendedKneeModel)
+			calculateKneeLimits(hipBuf, kneeBuf, rightLegTracker.getConfidenceLevel(), rightAnkleTracker.getConfidenceLevel());
 		
 		rightHipNode.localTransform.setRotation(hipBuf);
 		rightKneeNode.localTransform.setRotation(kneeBuf);
@@ -227,20 +257,35 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 			rightAnkleNode.localTransform.setRotation(kneeBuf);
 			rightFootNode.localTransform.setRotation(kneeBuf);
 		}
-		
-		// TODO Calculate waist node as some function between waist and hip rotations
+
+		if(extendedPelvisModel) {
+			// Average pelvis between two legs
+			leftHipNode.localTransform.getRotation(hipBuf);
+			rightHipNode.localTransform.getRotation(kneeBuf);
+			kneeBuf.slerp(hipBuf, 0.5f);
+			waistNode.localTransform.setRotation(kneeBuf);
+			// TODO : Use vectors to add like 50% of wasit tracker yaw to waist node to reduce drift and let user take weird poses
+		}
 	}
 	
-	// Knee basically has only 1 DoF (pitch), average yaw between knee and hip
+	// Knee basically has only 1 DoF (pitch), average yaw and roll between knee and hip
 	protected void calculateKneeLimits(Quaternion hipBuf, Quaternion kneeBuf, float hipConfidense, float kneeConfidense) {
-		hipBuf.toAngles(hipAngles);
-		kneeBuf.toAngles(kneeAngles);
+		ankleVector.set(0, -1, 0);
+		hipVector.set(0, -1, 0);
+		hipBuf.multLocal(hipVector);
+		kneeBuf.multLocal(ankleVector);
+		kneeRotation.angleBetweenVectors(hipVector, ankleVector); // Find knee angle
 		
-		hipAngles[1] = kneeAngles[1] = interpolateRadians(kneeLerpFactor, kneeAngles[1], hipAngles[1]);
-		//hipAngles[2] = kneeAngles[2] = interpolateRadians(kneeLerpFactor, kneeAngles[2], hipAngles[2]);
+		// Substract knee angle from knee rotation. With perfect leg and perfect
+		// sensors result should match hip rotation perfectly
+		kneeBuf.multLocal(kneeRotation.inverse());
 		
-		hipBuf.fromAngles(hipAngles);
-		kneeBuf.fromAngles(kneeAngles);
+		// Average knee and hip with a slerp
+		hipBuf.slerp(kneeBuf, 0.5f); // TODO : Use confidence to calculate changeAmt
+		kneeBuf.set(hipBuf);
+
+		// Return knee angle into knee rotation
+		kneeBuf.multLocal(kneeRotation);
 	}
 	
 	public static float normalizeRad(float angle) {
@@ -319,4 +364,35 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 			this.rightFootTracker.resetFull(referenceRotation);
 		}
 	}
+	
+	@Override
+	@VRServerThread
+	public void resetTrackersYaw() {
+		// Each tracker uses the tracker before it to adjust iteself,
+		// so trackers that don't need adjustments could be used too
+		super.resetTrackersYaw();
+		// Start with waist, it was reset in the parent
+		Quaternion referenceRotation = new Quaternion();
+		this.waistTracker.getRotation(referenceRotation);
+		
+		this.leftLegTracker.resetYaw(referenceRotation);
+		this.rightLegTracker.resetYaw(referenceRotation);
+		this.leftLegTracker.getRotation(referenceRotation);
+		
+		this.leftAnkleTracker.resetYaw(referenceRotation);
+		this.leftAnkleTracker.getRotation(referenceRotation);
+		
+		if(this.leftFootTracker != null) {
+			this.leftFootTracker.resetYaw(referenceRotation);
+		}
+
+		this.rightLegTracker.getRotation(referenceRotation);
+		
+		this.rightAnkleTracker.resetYaw(referenceRotation);
+		this.rightAnkleTracker.getRotation(referenceRotation);
+		
+		if(this.rightFootTracker != null) {
+			this.rightFootTracker.resetYaw(referenceRotation);
+		}
+	}
 }

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

@@ -14,7 +14,7 @@ import io.eiren.vr.trackers.Tracker;
 import io.eiren.vr.trackers.TrackerStatus;
 import io.eiren.vr.trackers.TrackerUtils;
 
-public class HumanSkeleonWithWaist extends HumanSkeleton {
+public class HumanSkeletonWithWaist extends HumanSkeleton {
 	
 	public static final float HEAD_SHIFT_DEFAULT = 0.1f;
 	public static final float NECK_LENGTH_DEFAULT = 0.1f;
@@ -58,7 +58,7 @@ public class HumanSkeleonWithWaist extends HumanSkeleton {
 	 */
 	protected float headShift = HEAD_SHIFT_DEFAULT;
 
-	public HumanSkeleonWithWaist(VRServer server, List<ComputedHumanPoseTracker> computedTrackers) {
+	public HumanSkeletonWithWaist(VRServer server, List<ComputedHumanPoseTracker> computedTrackers) {
 		List<Tracker> allTracekrs = server.getAllTrackers();
 		this.waistTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.WAIST, TrackerBodyPosition.CHEST);
 		this.chestTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.CHEST, TrackerBodyPosition.WAIST);
@@ -236,4 +236,18 @@ public class HumanSkeleonWithWaist extends HumanSkeleton {
 		
 		this.waistTracker.resetFull(referenceRotation);
 	}
+	
+	@Override
+	@VRServerThread
+	public void resetTrackersYaw() {
+		// Each tracker uses the tracker before it to adjust iteself,
+		// so trackers that don't need adjustments could be used too
+		Quaternion referenceRotation = new Quaternion();
+		server.hmdTracker.getRotation(referenceRotation);
+		
+		this.chestTracker.resetYaw(referenceRotation);
+		this.chestTracker.getRotation(referenceRotation);
+		
+		this.waistTracker.resetYaw(referenceRotation);
+	}
 }

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

@@ -0,0 +1,21 @@
+package io.eiren.vr.trackers;
+
+public class BnoTap {
+	
+	public final boolean doubleTap;
+	
+	public BnoTap(int tapBits) {
+		doubleTap = (tapBits & 0x40) > 0;
+	}
+	
+	@Override
+	public String toString() {
+		return "Tap{" + (doubleTap ? "double" : "") + "}";
+	}
+	
+	public enum TapAxis {
+		X,
+		Y,
+		Z;
+	}
+}

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

@@ -80,4 +80,8 @@ public class ComputedTracker implements Tracker {
 	public boolean userEditable() {
 		return false;
 	}
+
+	@Override
+	public void tick() {
+	}
 }

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

@@ -1,29 +0,0 @@
-package io.eiren.vr.trackers;
-
-public class IMUReferenceAdjustedTracker<T extends IMUTracker & TrackerWithTPS & TrackerWithBattery> extends ReferenceAdjustedTracker<T> implements TrackerWithTPS, TrackerWithBattery {
-	
-	public IMUReferenceAdjustedTracker(T tracker) {
-		super(tracker);
-	}
-
-	@Override
-	public float getBatteryLevel() {
-		return tracker.getBatteryLevel();
-	}
-
-	@Override
-	public float getBatteryVoltage() {
-		return tracker.getBatteryVoltage();
-	}
-
-	@Override
-	public float getTPS() {
-		return tracker.getTPS();
-	}
-
-	@Override
-	public void dataTick() {
-		tracker.dataTick();
-	}
-	
-}

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

@@ -1,5 +1,6 @@
 package io.eiren.vr.trackers;
 
+import com.jme3.math.FastMath;
 import com.jme3.math.Quaternion;
 import com.jme3.math.Vector3f;
 
@@ -9,11 +10,15 @@ import io.eiren.vr.processor.TrackerBodyPosition;
 
 public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery {
 	
+	public static final float MAX_MAG_CORRECTION_ACCURACY = 5 * FastMath.RAD_TO_DEG;
+	
 	public final Vector3f gyroVector = new Vector3f();
 	public final Vector3f accelVector = new Vector3f();
 	public final Vector3f magVector = new Vector3f();
 	public final Quaternion rotQuaternion = new Quaternion();
+	public final Quaternion rotMagQuaternion = new Quaternion();
 	protected final Quaternion rotAdjust = new Quaternion();
+	protected final Quaternion correction = new Quaternion();
 	protected TrackerMountingRotation mounting = null;
 	protected TrackerStatus status = TrackerStatus.OK;
 	
@@ -21,6 +26,11 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery {
 	protected final TrackersUDPServer server;
 	protected float confidence = 0;
 	protected float batteryVoltage = 0;
+	public int calibrationStatus = 0;
+	public int magCalibrationStatus = 0;
+	public float magnetometerAccuracy = 0;
+	protected boolean magentometerCalibrated = false;
+	public boolean hasNewCorrectionData = false;
 	
 	protected BufferedTimer timer = new BufferedTimer(1f);
 	public int ping = -1;
@@ -68,6 +78,18 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery {
 		}
 	}
 	
+	@Override
+	public void tick() {
+		if(magentometerCalibrated && hasNewCorrectionData) {
+			hasNewCorrectionData = false;
+			if(magnetometerAccuracy <= MAX_MAG_CORRECTION_ACCURACY) {
+				// Adjust gyro rotation to match magnetometer rotation only if magnetometer
+				// accuracy is within the parameters
+				calculateLiveMagnetometerCorrection();
+			}
+		}
+	}
+	
 	@Override
 	public String getName() {
 		return this.name;
@@ -82,9 +104,14 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery {
 	@Override
 	public boolean getRotation(Quaternion store) {
 		store.set(rotQuaternion);
+		//correction.mult(store, store); // Correction is not used now to preven accidental errors while debugging other things
 		store.multLocal(rotAdjust);
 		return true;
 	}
+	
+	public void getCorrection(Quaternion store) {
+		store.set(correction);
+	}
 
 	@Override
 	public TrackerStatus getStatus() {
@@ -130,10 +157,31 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery {
 
 	@Override
 	public void resetFull(Quaternion reference) {
+		resetYaw(reference);
 	}
 
+	/**
+	 * Does not perform actual gyro reset to reference, that's the task of
+	 * reference adjusted tracker. Only aligns gyro with magnetometer if
+	 * it's reliable
+	 */
 	@Override
 	public void resetYaw(Quaternion reference) {
+		if(magCalibrationStatus >= CalibrationAccuracy.HIGH.status) {
+			magentometerCalibrated = true;
+			// During calibration set correction to match magnetometer readings exactly
+			// TODO : Correct only yaw
+			correction.set(rotQuaternion).inverseLocal().multLocal(rotMagQuaternion);
+		}
+	}
+	
+	/**
+	 * Calculate correction between normal and magnetometer
+	 * readings up to accuracy threshold
+	 */
+	protected void calculateLiveMagnetometerCorrection() {
+		// TODO Magic, correct only yaw
+		// TODO Print "jump" length when correcing if it's more than 1 degree
 	}
 
 	@Override
@@ -150,4 +198,31 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery {
 	public boolean userEditable() {
 		return true;
 	}
+	
+	public enum CalibrationAccuracy {
+		
+		UNRELIABLE(0),
+		LOW(1),
+		MEDIUM(2),
+		HIGH(3),
+		;
+		
+		private static final CalibrationAccuracy[] byStatus = new CalibrationAccuracy[4];
+		public final int status;
+		
+		private CalibrationAccuracy(int status) {
+			this.status = status;
+		}
+		
+		public static CalibrationAccuracy getByStatus(int status) {
+			if(status < 0 || status > 3)
+				return null;
+			return byStatus[status];
+		}
+		
+		static {
+			for(CalibrationAccuracy ca : values())
+				byStatus[ca.status] = ca;
+		}
+	}
 }

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

@@ -45,6 +45,7 @@ public class ReferenceAdjustedTracker<E extends Tracker> implements Tracker {
 	 */
 	@Override
 	public void resetFull(Quaternion reference) {
+		tracker.resetFull(reference);
 		fixGyroscope();
 		
 		Quaternion sensorRotation = new Quaternion();
@@ -52,7 +53,7 @@ public class ReferenceAdjustedTracker<E extends Tracker> implements Tracker {
 		gyroFix.mult(sensorRotation, sensorRotation);
 		attachmentFix.set(sensorRotation).inverseLocal();
 		
-		resetYaw(reference);
+		fixYaw(reference);
 	}
 
 	/**
@@ -64,6 +65,11 @@ public class ReferenceAdjustedTracker<E extends Tracker> implements Tracker {
 	 */
 	@Override
 	public void resetYaw(Quaternion reference) {
+		tracker.resetYaw(reference);
+		fixYaw(reference);
+	}
+	
+	private void fixYaw(Quaternion reference) {
 		// Use only yaw HMD rotation
 		Quaternion targetTrackerRotation = new Quaternion(reference);
 		float[] angles = new float[3];
@@ -135,4 +141,9 @@ public class ReferenceAdjustedTracker<E extends Tracker> implements Tracker {
 	public void setBodyPosition(TrackerBodyPosition position) {
 		tracker.setBodyPosition(position);
 	}
+
+	@Override
+	public void tick() {
+		tracker.tick();
+	}
 }

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

@@ -25,6 +25,8 @@ public interface Tracker {
 	
 	public void resetYaw(Quaternion reference);
 	
+	public void tick();
+	
 	public TrackerBodyPosition getBodyPosition();
 	
 	public void setBodyPosition(TrackerBodyPosition position);

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

@@ -65,12 +65,16 @@ public class TrackersUDPServer extends Thread {
 			int boardType = -1;
 			int imuType = -1;
 			int firmwareBuild = -1;
-			StringBuilder sb = new StringBuilder();
+			StringBuilder firmware = new StringBuilder();
+			byte[] mac = new byte[6];
+			String macString = null;
 			if(data.remaining() > 0) {
 				if(data.remaining() > 3)
 					boardType = data.getInt();
 				if(data.remaining() > 3)
 					imuType = data.getInt();
+				if(data.remaining() > 3)
+					data.getInt(); // MCU TYPE
 				if(data.remaining() > 11) {
 					data.getInt(); // IMU info
 					data.getInt();
@@ -78,19 +82,25 @@ public class TrackersUDPServer extends Thread {
 				}
 				if(data.remaining() > 3)
 					firmwareBuild = data.getInt();
-				while(true) {
-					if(data.remaining() == 0)
-						break;
+				int length = 0;
+				if(data.remaining() > 0)
+					length = data.get() & 0xFF; // firmware version length is 1 longer than that because it's nul-terminated
+				while(length > 0 && data.remaining() != 0) {
 					char c = (char) data.get();
 					if(c == 0)
 						break;
-					sb.append(c);
+					firmware.append(c);
+					length--;
+				}
+				if(data.remaining() > mac.length) {
+					data.get(mac);
+					macString = String.format("%02X:%02X:%02X:%02X:%02X:%02X", mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
 				}
 			}
-			if(sb.length() == 0)
-				sb.append("owoTrack");
+			if(firmware.length() == 0)
+				firmware.append("owoTrack");
 			IMUTracker imu = new IMUTracker("udp:/" + handshakePacket.getAddress().toString(), this);
-			IMUReferenceAdjustedTracker<IMUTracker> adjustedTracker = new IMUReferenceAdjustedTracker<>(imu);
+			ReferenceAdjustedTracker<IMUTracker> adjustedTracker = new ReferenceAdjustedTracker<>(imu);
 			trackersConsumer.accept(adjustedTracker);
 			sensor = new TrackerConnection(imu, addr);
 			int i = 0;
@@ -99,7 +109,7 @@ public class TrackersUDPServer extends Thread {
 				trackers.add(sensor);
 				trackersMap.put(addr, sensor);
 			}
-			System.out.println("[TrackerServer] Sensor " + i + " added with address " + addr + ". Board type: " + boardType + ", imu type: " + imuType + ", firmware: " + sb + " (" + firmwareBuild + ")");
+			System.out.println("[TrackerServer] Sensor " + i + " added with address " + addr + ". Board type: " + boardType + ", imu type: " + imuType + ", firmware: " + firmware + " (" + firmwareBuild + "), mac: " + macString);
 		}
 		sensor.tracker.setStatus(TrackerStatus.OK);
         socket.send(new DatagramPacket(HANDSHAKE_BUFFER, HANDSHAKE_BUFFER.length, handshakePacket.getAddress(), handshakePacket.getPort()));
@@ -109,7 +119,7 @@ public class TrackersUDPServer extends Thread {
 		System.out.println("[TrackerServer] Setting up auxilary sensor for " + connection.tracker.getName());
 		IMUTracker imu = new IMUTracker(connection.tracker.getName() + "/1", this);
 		connection.secondTracker = imu;
-		IMUReferenceAdjustedTracker<IMUTracker> adjustedTracker = new IMUReferenceAdjustedTracker<>(imu);
+		ReferenceAdjustedTracker<IMUTracker> adjustedTracker = new ReferenceAdjustedTracker<>(imu);
 		trackersConsumer.accept(adjustedTracker);
 		System.out.println("[TrackerServer] Sensor added with address " + imu.getName());
 	}
@@ -129,12 +139,13 @@ public class TrackersUDPServer extends Thread {
 					socket.receive(recieve);
 					bb.rewind();
 
-					TrackerConnection sensor;
+					TrackerConnection connection;
+					IMUTracker tracker = null;
 					synchronized(trackers) {
-						sensor = trackersMap.get(recieve.getSocketAddress());
+						connection = trackersMap.get(recieve.getSocketAddress());
 					}
-					if(sensor != null)
-						sensor.lastPacket = System.currentTimeMillis();
+					if(connection != null)
+						connection.lastPacket = System.currentTimeMillis();
 					int packetId;
 					switch(packetId = bb.getInt()) {
 					case 0:
@@ -144,90 +155,136 @@ public class TrackersUDPServer extends Thread {
 						break;
 					case 1: // PACKET_ROTATION
 					case 16: // PACKET_ROTATION_2
-						if(sensor == null)
+						if(connection == null)
 							break;
 						bb.getLong();
 						buf.set(bb.getFloat(), bb.getFloat(), bb.getFloat(), bb.getFloat());
 						offset.mult(buf, buf);
-						IMUTracker tracker;
 						if(packetId == 1) {
-							tracker = sensor.tracker;
+							tracker = connection.tracker;
 						} else {
-							tracker = sensor.secondTracker;
+							tracker = connection.secondTracker;
 						}
 						if(tracker == null)
 							break;
 						tracker.rotQuaternion.set(buf);
 						tracker.dataTick();
 						break;
-					case 2:
-						if(sensor == null)
+					case 17: // PACKET_ROTATION_DATA
+						if(connection == null)
 							break;
 						bb.getLong();
-						sensor.tracker.gyroVector.set(bb.getFloat(), bb.getFloat(), bb.getFloat());
+						int sensorId = bb.get() & 0xFF;
+						if(sensorId == 0) {
+							tracker = connection.tracker;
+						} else if(sensorId == 1) {
+							tracker = connection.secondTracker;
+						}
+						if(tracker == null)
+							break;
+						
+						int dataType = bb.get() & 0xFF;
+						buf.set(bb.getFloat(), bb.getFloat(), bb.getFloat(), bb.getFloat());
+						offset.mult(buf, buf);
+						int calibrationInfo = bb.get() & 0xFF;
+						
+						switch(dataType) {
+						case 1: // DATA_TYPE_NORMAL
+							tracker.rotQuaternion.set(buf);
+							tracker.calibrationStatus = calibrationInfo;
+							tracker.dataTick();
+							break;
+						case 2: // DATA_TYPE_CORRECTION
+							tracker.rotMagQuaternion.set(buf);
+							tracker.magCalibrationStatus = calibrationInfo;
+							tracker.hasNewCorrectionData = true;
+							break;
+						}
+						break;
+					case 18: // PACKET_MAGENTOMETER_ACCURACY
+						if(connection == null)
+							break;
+						bb.getLong();
+						sensorId = bb.get() & 0xFF;
+						if(sensorId == 0) {
+							tracker = connection.tracker;
+						} else if(sensorId == 1) {
+							tracker = connection.secondTracker;
+						}
+						if(tracker == null)
+							break;
+						float accuracyInfo = bb.getFloat();
+						tracker.magnetometerAccuracy = accuracyInfo;
+						// TODO
+						break;
+					case 2:
+						if(connection == null)
+							break;
+						bb.getLong();
+						connection.tracker.gyroVector.set(bb.getFloat(), bb.getFloat(), bb.getFloat());
 						break;
 					case 4:
-						if(sensor == null)
+						if(connection == null)
 							break;
 						bb.getLong();
 						float x = bb.getFloat();
 						float z = bb.getFloat();
 						float y = bb.getFloat();
-						sensor.tracker.accelVector.set(x, y, z);
+						connection.tracker.accelVector.set(x, y, z);
 						break;
 					case 5:
-						if(sensor == null)
+						if(connection == null)
 							break;
 						bb.getLong();
 						x = bb.getFloat();
 						z = bb.getFloat();
 						y = bb.getFloat();
-						sensor.tracker.magVector.set(x, y, z);
+						connection.tracker.magVector.set(x, y, z);
 						break;
 					case 6: // PACKET_RAW_CALIBRATION_DATA
-						if(sensor == null)
+						if(connection == null)
 							break;
 						bb.getLong();
 						//sensor.rawCalibrationData.add(new double[] {bb.getInt(), bb.getInt(), bb.getInt(), bb.getInt(), bb.getInt(), bb.getInt()});
 						break;
 					case 7: // PACKET_GYRO_CALIBRATION_DATA
-						if(sensor == null)
+						if(connection == null)
 							break;
 						bb.getLong();
 						//sensor.gyroCalibrationData = new double[] {bb.getFloat(), bb.getFloat(), bb.getFloat()};
 						break;
 					case 8: // PACKET_CONFIG
-						if(sensor == null)
+						if(connection == null)
 							break;
 						bb.getLong();
 						MPUTracker.ConfigurationData data = new MPUTracker.ConfigurationData(bb);
-						Consumer<String> dataConsumer = calibrationDataRequests.remove(sensor.tracker);
+						Consumer<String> dataConsumer = calibrationDataRequests.remove(connection.tracker);
 						if(dataConsumer != null) {
 							dataConsumer.accept(data.toTextMatrix());
 						}
 						break;
 					case 9: // PACKET_RAW_MAGENTOMETER
-						if(sensor == null)
+						if(connection == null)
 							break;
 						bb.getLong();
 						float mx = bb.getFloat();
 						float my = bb.getFloat();
 						float mz = bb.getFloat();
-						sensor.tracker.confidence = (float) Math.sqrt(mx * mx + my * my + mz * mz);
+						connection.tracker.confidence = (float) Math.sqrt(mx * mx + my * my + mz * mz);
 						break;
 					case 10: // PACKET_PING_PONG:
-						if(sensor == null)
+						if(connection == null)
 							break;
 						int pingId = bb.getInt();
-						if(sensor.lastPingPacketId == pingId) {
-							tracker = sensor.tracker;
-							tracker.ping = (int) (System.currentTimeMillis() - sensor.lastPingPacketTime) / 2;
+						if(connection.lastPingPacketId == pingId) {
+							tracker = connection.tracker;
+							tracker.ping = (int) (System.currentTimeMillis() - connection.lastPingPacketTime) / 2;
 						}
 						break;
 					case 11: // PACKET_SERIAL
-						if(sensor == null)
+						if(connection == null)
 							break;
-						tracker = sensor.tracker;
+						tracker = connection.tracker;
 						bb.getLong();
 						int length = bb.getInt();
 						for(int i = 0; i < length; ++i) {
@@ -243,44 +300,51 @@ public class TrackersUDPServer extends Thread {
 						}
 						break;
 					case 12: // PACKET_BATTERY_VOLTAGE
-						if(sensor == null)
+						if(connection == null)
 							break;
-						tracker = sensor.tracker;
+						tracker = connection.tracker;
 						bb.getLong();
 						tracker.setBatteryVoltage(bb.getFloat());
 						break;
 					case 13: // PACKET_TAP
-						if(sensor == null)
+						if(connection == null)
 							break;
-						tracker = sensor.tracker;
+						tracker = connection.tracker;
 						bb.getLong();
-						byte tap = bb.get();
-						System.out.println("[TrackerServer] Tap packet received from " + tracker.getName() + ": b" + Integer.toBinaryString(tap));
+						sensorId = bb.get() & 0xFF;
+						if(sensorId == 0) {
+							tracker = connection.tracker;
+						} else if(sensorId == 1) {
+							tracker = connection.secondTracker;
+						}
+						int tap = bb.get() & 0xFF;
+						BnoTap tapObj = new BnoTap(tap);
+						System.out.println("[TrackerServer] Tap packet received from " + tracker.getName() + "/" + sensorId + ": " + tapObj  + " (b" + Integer.toBinaryString(tap) + ")");
 						break;
 					case 14: // PACKET_RESET_REASON
 						bb.getLong();
 						byte reason = bb.get();
 						System.out.println("[TrackerServer] Reset recieved from " + recieve.getSocketAddress() + ": " + reason);
-						if(sensor == null)
+						if(connection == null)
 							break;
-						tracker = sensor.tracker;
+						tracker = connection.tracker;
 						tracker.setStatus(TrackerStatus.ERROR);
 						break;
 					case 15: // PACKET_SENSOR_INFO
-						if(sensor == null)
+						if(connection == null)
 							break;
 						bb.getLong();
-						int sensorId = bb.get() & 0xFF;
+						sensorId = bb.get() & 0xFF;
 						int sensorStatus = bb.get() & 0xFF;
-						if(sensorId == 1 && sensorStatus == 1 && sensor.secondTracker == null) {
-							setUpAuxialrySensor(sensor);
+						if(sensorId == 1 && sensorStatus == 1 && connection.secondTracker == null) {
+							setUpAuxialrySensor(connection);
 						}
 						bb.rewind();
 						bb.putInt(15);
 						bb.put((byte) sensorId);
 						bb.put((byte) sensorStatus);
-						socket.send(new DatagramPacket(rcvBuffer, bb.position(), sensor.address));
-						System.out.println("[TrackerServer] Sensor info for " + sensor.tracker.getName() + "/" + sensorId + ": " + sensorStatus);
+						socket.send(new DatagramPacket(rcvBuffer, bb.position(), connection.address));
+						System.out.println("[TrackerServer] Sensor info for " + connection.tracker.getName() + "/" + sensorId + ": " + sensorStatus);
 						break;
 					default:
 						System.out.println("[TrackerServer] Unknown data received: " + packetId + " from " + recieve.getSocketAddress());