Added basic GUI

Added more config loading and saving to human skeleton
Added smoothing of tracker reading to preven jitter
Adjust skeleton

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

@@ -0,0 +1,89 @@
+package io.eiren.gui;
+
+
+import java.awt.Container;
+import java.awt.event.MouseEvent;
+
+import javax.swing.BoxLayout;
+import javax.swing.JButton;
+import javax.swing.JFrame;
+import javax.swing.JLabel;
+import javax.swing.JTextArea;
+import javax.swing.border.EmptyBorder;
+import javax.swing.event.MouseInputAdapter;
+
+import io.eiren.util.ann.AWTThread;
+import io.eiren.vr.trackers.CalibratingTracker;
+import io.eiren.vr.trackers.Tracker;
+
+public class CalibrationWindow extends JFrame {
+	
+	public final Tracker tracker;
+	private JTextArea currentCalibration;
+	private JTextArea newCalibration;
+	private JButton calibrateButton;
+	
+	public CalibrationWindow(Tracker t) {
+		super(t.getName() + " calibration");
+		this.tracker = t;
+		getContentPane().setLayout(new BoxLayout(getContentPane(), BoxLayout.LINE_AXIS));
+		
+		build();
+	}
+	
+	public void currentCalibrationRecieved(String str) {
+		java.awt.EventQueue.invokeLater(() -> {
+			currentCalibration.setText(str);
+			pack();
+		});
+	}
+	
+	public void newCalibrationRecieved(String str) {
+		java.awt.EventQueue.invokeLater(() -> {
+			calibrateButton.setText("Calibrate");
+			newCalibration.setText(str);
+			pack();
+		});
+	}
+	
+	@AWTThread
+	private void build() {
+		Container pane = getContentPane();
+		
+		pane.add(calibrateButton = new JButton("Calibrate") {{
+			addMouseListener(new MouseInputAdapter() {
+				@Override
+				public void mouseClicked(MouseEvent e) {
+					calibrateButton.setText("Calibrating...");
+					((CalibratingTracker) tracker).startCalibration(CalibrationWindow.this::newCalibrationRecieved);
+				}
+			});
+		}});
+
+		pane.add(new EJBox(BoxLayout.PAGE_AXIS) {{
+			setBorder(new EmptyBorder(i(5)));
+			add(new JLabel("Current calibration"));
+			add(currentCalibration = new JTextArea(10, 25));
+			
+			((CalibratingTracker) tracker).requestCalibrationData(CalibrationWindow.this::currentCalibrationRecieved);
+		}});
+		pane.add(new EJBox(BoxLayout.PAGE_AXIS) {{
+			setBorder(new EmptyBorder(i(5)));
+			add(new JLabel("New calibration"));
+			add(newCalibration = new JTextArea(10, 25));
+		}});
+		
+		
+		// Pack and display
+		pack();
+		setLocationRelativeTo(null);
+		setVisible(true);
+		java.awt.EventQueue.invokeLater(new Runnable() {
+			@Override
+			public void run() {
+				toFront();
+				repaint();
+			}
+		});
+	}
+}

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

@@ -0,0 +1,10 @@
+package io.eiren.gui;
+
+import java.awt.GridBagLayout;
+
+public class EJBag extends EJPanel {
+	
+	public EJBag() {
+		super(new GridBagLayout());
+	}
+}

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

@@ -0,0 +1,11 @@
+package io.eiren.gui;
+
+import javax.swing.BoxLayout;
+
+public class EJBox extends EJPanel {
+	
+	public EJBox(int layout) {
+		super();
+		setLayout(new BoxLayout(this, layout));
+	}
+}

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

@@ -0,0 +1,104 @@
+package io.eiren.gui;
+
+import java.awt.Component;
+import java.awt.Dimension;
+import java.awt.GridBagConstraints;
+import java.awt.Insets;
+import java.awt.LayoutManager;
+
+import javax.swing.Box;
+import javax.swing.JPanel;
+
+public abstract class EJPanel extends JPanel {
+	
+	public static boolean NEEDS_DOWNSCALE = false;
+	public static float DOWNSCALE_FACTOR = 0.75f;
+	
+	public EJPanel() {
+		super();
+	}
+	
+	public EJPanel(LayoutManager manager) {
+		super(manager);
+	}
+	
+	public static void s(Component c, int width, int height) {
+		if(NEEDS_DOWNSCALE) {
+			width = (int) Math.ceil(width * DOWNSCALE_FACTOR);
+			height = (int) Math.ceil(height * DOWNSCALE_FACTOR);
+		}
+		c.setSize(width, height);
+		Dimension d = new Dimension(width, height);
+		c.setPreferredSize(d);
+		c.setMaximumSize(d);
+		c.setMinimumSize(d);
+	}
+	
+	public static void minWidth(Component c, int width, int height) {
+		if(NEEDS_DOWNSCALE) {
+			height = (int) Math.ceil(height * DOWNSCALE_FACTOR);
+			width = (int) Math.ceil(width * DOWNSCALE_FACTOR);
+		}
+		c.setPreferredSize(new Dimension(Short.MAX_VALUE, height));
+		c.setMaximumSize(new Dimension(Short.MAX_VALUE, height));
+		c.setMinimumSize(new Dimension(width, height));
+	}
+	
+	public static void minHeight(Component c, int width, int height) {
+		if(NEEDS_DOWNSCALE) {
+			height = (int) Math.ceil(height * DOWNSCALE_FACTOR);
+			width = (int) Math.ceil(width * DOWNSCALE_FACTOR);
+		}
+		c.setPreferredSize(new Dimension(width, Short.MAX_VALUE));
+		c.setMaximumSize(new Dimension(width, Short.MAX_VALUE));
+		c.setMinimumSize(new Dimension(width, height));
+	}
+	
+	public static GridBagConstraints c(int x, int y) {
+		GridBagConstraints c = new GridBagConstraints();
+		c.gridx = x;
+		c.gridy = y;
+		return c;
+	}
+	
+	public static GridBagConstraints c(int x, int y, int padding) {
+		GridBagConstraints c = new GridBagConstraints();
+		c.gridx = x;
+		c.gridy = y;
+		c.insets = new Insets(padding, padding, padding, padding);
+		return c;
+	}
+	
+	public static GridBagConstraints c(int x, int y, int padding, int anchor) {
+		GridBagConstraints c = new GridBagConstraints();
+		c.gridx = x;
+		c.gridy = y;
+		c.insets = new Insets(padding, padding, padding, padding);
+		c.anchor = anchor;
+		return c;
+	}
+	
+	public static GridBagConstraints c(int x, int y, Insets insets) {
+		GridBagConstraints c = new GridBagConstraints();
+		c.gridx = x;
+		c.gridy = y;
+		c.insets = insets;
+		return c;
+	}
+	
+	public static Insets i(int s) {
+		return new Insets(s, s, s, s);
+	}
+
+	public static Insets i(int h, int v) {
+		return new Insets(v, h, v, h);
+	}
+	
+	public static Component padding(int width, int height) {
+		return Box.createRigidArea(new Dimension(width, height));
+	}
+	
+	public static int fontSize(int baseSize) {
+		return NEEDS_DOWNSCALE ? (int) Math.ceil(baseSize * DOWNSCALE_FACTOR) : baseSize;
+	}
+}

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

@@ -0,0 +1,8 @@
+package io.eiren.gui;
+
+import javax.swing.JLabel;
+
+public class EJlabel extends JLabel {
+	
+	
+}

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

@@ -0,0 +1,105 @@
+package io.eiren.gui;
+
+import java.awt.GridBagConstraints;
+import java.util.List;
+
+import javax.swing.JLabel;
+
+import com.jme3.math.FastMath;
+import com.jme3.math.Quaternion;
+import com.jme3.math.Vector3f;
+
+import io.eiren.util.StringUtils;
+import io.eiren.util.ann.ThreadSafe;
+import io.eiren.util.ann.VRServerThread;
+import io.eiren.util.collections.FastList;
+import io.eiren.vr.VRServer;
+import io.eiren.vr.processor.HumanSkeleton;
+import io.eiren.vr.processor.TransformNode;
+
+public class SkeletonList extends EJBag {
+
+	Quaternion q = new Quaternion();
+	Vector3f v = new Vector3f();
+	float[] angles = new float[3];
+	
+	private final VRServer server;
+	private final VRServerGUI gui;
+	private final List<NodeStatus> nodes = new FastList<>();
+	
+	public SkeletonList(VRServer server, VRServerGUI gui) {
+		super();
+		this.server = server;
+		this.gui = gui;
+
+		setAlignmentY(TOP_ALIGNMENT);
+		server.addSkeletonUpdatedCallback(this::skeletonUpdated);
+	}
+	
+	@ThreadSafe
+	public void skeletonUpdated(HumanSkeleton newSkeleton) {
+		java.awt.EventQueue.invokeLater(() -> {
+			removeAll();
+			nodes.clear();
+			
+			add(new JLabel("Joint"), c(0, 0, 2));
+			add(new JLabel("X"), c(1, 0, 2));
+			add(new JLabel("Y"), c(2, 0, 2));
+			add(new JLabel("Z"), c(3, 0, 2));
+			add(new JLabel("Pitch"), c(4, 0, 2));
+			add(new JLabel("Yaw"), c(5, 0, 2));
+			add(new JLabel("Roll"), c(6, 0, 2));
+			
+			newSkeleton.getRootNode().depthFirstTraversal((node) -> {
+				int n = nodes.size();
+				nodes.add(new NodeStatus(node, n + 1));
+			});
+			
+			
+			gui.refresh();
+		});
+	}
+	
+	@VRServerThread
+	public void updateBones() {
+		java.awt.EventQueue.invokeLater(() -> {
+			for(int i = 0; i < nodes.size(); ++i)
+				nodes.get(i).update();
+		});
+	}
+	
+	private class NodeStatus {
+
+		TransformNode n;
+		JLabel x;
+		JLabel y;
+		JLabel z;
+		JLabel a1;
+		JLabel a2;
+		JLabel a3;
+		
+		public NodeStatus(TransformNode node, int n) {
+			this.n = node;
+			add(new JLabel(node.getName()), c(0, n, 2, GridBagConstraints.FIRST_LINE_START));
+			add(x = new JLabel("0"), c(1, n, 2, GridBagConstraints.FIRST_LINE_START));
+			add(y = new JLabel("0"), c(2, n, 2, GridBagConstraints.FIRST_LINE_START));
+			add(z = new JLabel("0"), c(3, n, 2, GridBagConstraints.FIRST_LINE_START));
+			add(a1 = new JLabel("0"), c(4, n, 2, GridBagConstraints.FIRST_LINE_START));
+			add(a2 = new JLabel("0"), c(5, n, 2, GridBagConstraints.FIRST_LINE_START));
+			add(a3 = new JLabel("0"), c(6, n, 2, GridBagConstraints.FIRST_LINE_START));
+		}
+		
+		public void update() {
+			n.worldTransform.getTranslation(v);
+			n.worldTransform.getRotation(q);
+			q.toAngles(angles);
+			
+			x.setText(StringUtils.prettyNumber(v.x, 2));
+			y.setText(StringUtils.prettyNumber(v.y, 2));
+			z.setText(StringUtils.prettyNumber(v.z, 2));
+			a1.setText(StringUtils.prettyNumber(angles[0] * FastMath.RAD_TO_DEG, 0));
+			a2.setText(StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 0));
+			a3.setText(StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0));
+		}
+	}
+}

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

@@ -0,0 +1,137 @@
+package io.eiren.gui;
+
+import java.awt.GridBagConstraints;
+import java.awt.event.MouseEvent;
+import java.util.List;
+
+import javax.swing.BoxLayout;
+import javax.swing.JButton;
+import javax.swing.JLabel;
+import javax.swing.event.MouseInputAdapter;
+
+import com.jme3.math.FastMath;
+import com.jme3.math.Quaternion;
+import com.jme3.math.Vector3f;
+
+import io.eiren.util.StringUtils;
+import io.eiren.util.ann.AWTThread;
+import io.eiren.util.ann.ThreadSafe;
+import io.eiren.util.ann.VRServerThread;
+import io.eiren.util.collections.FastList;
+import io.eiren.vr.VRServer;
+import io.eiren.vr.trackers.CalibratingTracker;
+import io.eiren.vr.trackers.IMUTracker;
+import io.eiren.vr.trackers.Tracker;
+import io.eiren.vr.trackers.TrackerConfig;
+
+public class TrackersList extends EJBag {
+	
+	Quaternion q = new Quaternion();
+	Vector3f v = new Vector3f();
+	float[] angles = new float[3];
+	
+	private List<TrackerRow> trackers = new FastList<>();
+	
+	private final VRServer server;
+	private final VRServerGUI gui;
+
+	public TrackersList(VRServer server, VRServerGUI gui) {
+		super();
+		this.server = server;
+		this.gui = gui;
+
+		setAlignmentY(TOP_ALIGNMENT);
+		add(new JLabel("Tracker"), c(0, 0, 2));
+		add(new JLabel("Designation"), c(1, 0, 2));
+		add(new JLabel("X"), c(2, 0, 2));
+		add(new JLabel("Y"), c(3, 0, 2));
+		add(new JLabel("Z"), c(4, 0, 2));
+		add(new JLabel("Pitch"), c(5, 0, 2));
+		add(new JLabel("Yaw"), c(6, 0, 2));
+		add(new JLabel("Roll"), c(7, 0, 2));
+		add(new JLabel("Status"), c(8, 0, 2));
+		add(new JLabel("TPS"), c(9, 0, 2));
+		
+		server.addNewTrackerConsumer(this::newTrackerAdded);
+	}
+	
+	@VRServerThread
+	public void updateTrackers() {
+		java.awt.EventQueue.invokeLater(() -> {
+			for(int i = 0; i < trackers.size(); ++i)
+				trackers.get(i).update();
+		});
+	}
+	
+	@ThreadSafe
+	public void newTrackerAdded(Tracker t) {
+		java.awt.EventQueue.invokeLater(() -> {
+			final int n = trackers.size();
+			TrackerConfig cfg = server.getTrackerConfig(t);
+			
+			trackers.add(new TrackerRow(t, n + 1));
+			if(cfg.designation != null)
+				add(new JLabel(cfg.designation), c(1, n + 1, 2));
+			if(t instanceof CalibratingTracker) {
+				add(new JButton("Calibrate") {{
+					addMouseListener(new MouseInputAdapter() {
+						@Override
+						public void mouseClicked(MouseEvent e) {
+							new CalibrationWindow(t);
+						}
+					});
+				}}, c(10, n + 1, 2));
+			}
+			
+			gui.refresh();
+		});
+	}
+	
+	private class TrackerRow {
+		
+		Tracker t;
+		JLabel x;
+		JLabel y;
+		JLabel z;
+		JLabel a1;
+		JLabel a2;
+		JLabel a3;
+		JLabel status;
+		JLabel tps;
+		
+		@AWTThread
+		public TrackerRow(Tracker t, int n) {
+			this.t = t;
+			add(new JLabel(t.getName()), c(0, n, 2, GridBagConstraints.FIRST_LINE_START));
+			add(x = new JLabel("0"), c(2, n, 2, GridBagConstraints.FIRST_LINE_START));
+			add(y = new JLabel("0"), c(3, n, 2, GridBagConstraints.FIRST_LINE_START));
+			add(z = new JLabel("0"), c(4, n, 2, GridBagConstraints.FIRST_LINE_START));
+			add(a1 = new JLabel("0"), c(5, n, 2, GridBagConstraints.FIRST_LINE_START));
+			add(a2 = new JLabel("0"), c(6, n, 2, GridBagConstraints.FIRST_LINE_START));
+			add(a3 = new JLabel("0"), c(7, n, 2, GridBagConstraints.FIRST_LINE_START));
+			add(status = new JLabel(t.getStatus().toString()), c(8, n, 2, GridBagConstraints.FIRST_LINE_START));
+			if(t instanceof IMUTracker) {
+				add(tps = new JLabel("0"), c(9, n, 2, GridBagConstraints.FIRST_LINE_START));
+			}
+		}
+
+		@AWTThread
+		public void update() {
+			t.getRotation(q);
+			t.getPosition(v);
+			q.toAngles(angles);
+			
+			x.setText(StringUtils.prettyNumber(v.x, 2));
+			y.setText(StringUtils.prettyNumber(v.y, 2));
+			z.setText(StringUtils.prettyNumber(v.z, 2));
+			a1.setText(StringUtils.prettyNumber(angles[0] * FastMath.RAD_TO_DEG, 0));
+			a2.setText(StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 0));
+			a3.setText(StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0));
+			status.setText(t.getStatus().toString());
+			
+			if(t instanceof IMUTracker) {
+				tps.setText(StringUtils.prettyNumber(((IMUTracker) t).getTPS(), 1));
+			}
+		}
+	}
+}

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

@@ -0,0 +1,142 @@
+package io.eiren.gui;
+
+import javax.swing.*;
+import javax.swing.border.EmptyBorder;
+import javax.swing.event.MouseInputAdapter;
+
+import io.eiren.util.ann.AWTThread;
+import io.eiren.vr.VRServer;
+
+import java.awt.Container;
+import java.awt.event.MouseEvent;
+import java.util.TimerTask;
+
+import static javax.swing.BoxLayout.PAGE_AXIS;
+import static javax.swing.BoxLayout.LINE_AXIS;
+
+public class VRServerGUI extends JFrame {
+	
+	public final VRServer server;
+	private final TrackersList trackersList;
+	private final SkeletonList skeletonList;
+	private java.util.Timer timer = new java.util.Timer();
+	private JButton resetButton;
+	
+	@AWTThread
+	public VRServerGUI(VRServer server) {
+		super("SlimeVR Server");
+		increaseFontSize();
+		
+		this.server = server;
+		setDefaultCloseOperation(EXIT_ON_CLOSE);
+		getContentPane().setLayout(new BoxLayout(getContentPane(), PAGE_AXIS));
+		
+		this.trackersList = new TrackersList(server, this);
+		this.skeletonList = new SkeletonList(server, this);
+		
+		build();
+	}
+	
+	public void refresh() {
+		// Pack and display
+		pack();
+		setVisible(true);
+		java.awt.EventQueue.invokeLater(new Runnable() {
+			@Override
+			public void run() {
+				toFront();
+				repaint();
+			}
+		});
+	}
+	
+	@AWTThread
+	private void build() {
+		Container pane = getContentPane();
+		
+		pane.removeAll();
+		
+		pane.add(new EJBox(LINE_AXIS) {{
+			setBorder(new EmptyBorder(i(5)));
+			add(resetButton = new JButton("RESET") {{
+				addMouseListener(new MouseInputAdapter() {
+					@Override
+					public void mouseClicked(MouseEvent e) {
+						reset();
+					}
+				});
+			}});
+		}});
+		
+		pane.add(new EJBox(LINE_AXIS) {{
+			setBorder(new EmptyBorder(i(5)));
+			add(new EJBox(PAGE_AXIS) {{
+				setAlignmentY(TOP_ALIGNMENT);
+				
+				add(new JLabel("Trackers"));
+				add(trackersList);
+			}});
+
+			add(new EJBox(PAGE_AXIS) {{
+				setAlignmentY(TOP_ALIGNMENT);
+				add(new JLabel("Sekelton"));
+				add(skeletonList);
+			}});
+		}});
+		
+		refresh();
+		setLocationRelativeTo(null);
+		
+		server.addOnTick(trackersList::updateTrackers);
+		server.addOnTick(skeletonList::updateBones);
+	}
+	
+	private static void increaseFontSize() {
+		java.util.Enumeration<Object> keys = UIManager.getDefaults().keys();
+		while(keys.hasMoreElements()) {
+			Object key = keys.nextElement();
+			Object value = UIManager.get(key);
+			if(value instanceof javax.swing.plaf.FontUIResource) {
+				javax.swing.plaf.FontUIResource f = (javax.swing.plaf.FontUIResource) value;
+				javax.swing.plaf.FontUIResource f2 = new javax.swing.plaf.FontUIResource(f.deriveFont(f.getSize() * 2f));
+				UIManager.put(key, f2);
+			}
+		}
+	}
+	
+	@AWTThread
+	private void reset() {
+		resetButton.setText("5");
+		timer.schedule(new TimerTask() {
+			@Override
+			public void run() {
+				resetButton.setText("4");
+				timer.schedule(new TimerTask() {
+					@Override
+					public void run() {
+						resetButton.setText("3");
+						timer.schedule(new TimerTask() {
+							@Override
+							public void run() {
+								resetButton.setText("2");
+								timer.schedule(new TimerTask() {
+									@Override
+									public void run() {
+										resetButton.setText("1");
+										timer.schedule(new TimerTask() {
+											@Override
+											public void run() {
+												server.resetTrackers();
+												resetButton.setText("RESET");
+											}
+										}, 1000);
+									}
+								}, 1000);
+							}
+						}, 1000);
+					}
+				}, 1000);
+			}
+		}, 1000);
+	}
+}

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

@@ -0,0 +1,28 @@
+package io.eiren.vr;
+
+import java.io.File;
+
+import io.eiren.gui.VRServerGUI;
+import io.eiren.util.logging.LogManager;
+
+
+public class Main {
+	
+	public static void main(String[] args) {
+		System.setProperty("awt.useSystemAAFontSettings", "on");
+		System.setProperty("swing.aatext", "true");
+		
+		File dir = new File("").getAbsoluteFile();
+		try {
+			LogManager.initialize(new File(dir, "logs/"), dir);
+		} catch(Exception e1) {
+			e1.printStackTrace();
+		}
+		
+		VRServer vrServer = new VRServer();
+		vrServer.start();
+		
+		new VRServerGUI(vrServer);
+	}
+	
+}

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

@@ -38,10 +38,12 @@ public class VRServer extends Thread {
 	public final YamlFile config = new YamlFile();
 	public final HMDTracker hmdTracker;
 	private final List<Consumer<Tracker>> newTrackersConsumers = new FastList<>();
+	private final List<Runnable> onTick = new FastList<>();
 	
 	public VRServer() {
 		super("VRServer");
 		hmdTracker = new HMDTracker("HMD");
+		hmdTracker.position.set(0, 1.8f, 0); // Set starting position for easier debugging
 		humanPoseProcessor = new HumanPoseProcessor(this, hmdTracker);
 		List<? extends Tracker> shareTrackers = humanPoseProcessor.getComputedTrackers();
 		driverBridge = new NamedPipeVRBridge(hmdTracker, shareTrackers);
@@ -80,6 +82,10 @@ public class VRServer extends Thread {
 		}
 	}
 	
+	public void addOnTick(Runnable runnable) {
+		this.onTick.add(runnable);
+	}
+	
 	@ThreadSafe
 	public void addNewTrackerConsumer(Consumer<Tracker> consumer) {
 		queueTask(() -> {
@@ -146,6 +152,10 @@ public class VRServer extends Thread {
 				task.run();
 			} while(true);
 			
+			for(int i = 0; i < onTick.size(); ++i) {
+				this.onTick.get(i).run();
+			}
+			
 			humanPoseProcessor.update();
 			
 			final long time = System.currentTimeMillis() - start;

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

@@ -26,15 +26,15 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 	/**
 	 * Distance between centers of both hips
 	 */
-	protected float hipsWidth = 0.3f;
+	protected float hipsWidth = 0.22f;
 	/**
 	 * Length from waist to knees
 	 */
-	protected float hipLength = 0.86f;
+	protected float hipsLength = 0.46f;
 	/**
 	 * Distance from waist to ankle
 	 */
-	protected float ankleLength = 0.42f;
+	protected float ankleLength = 0.4f;
 
 	public HumanSekeletonWithLegs(VRServer server, Map<TrackerBodyPosition, ? extends Tracker> trackers, List<ComputedHumanPoseTracker> computedTrackers) {
 		super(server, trackers.get(TrackerBodyPosition.WAIST), computedTrackers);
@@ -55,6 +55,9 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 		computedRightAnkleTracker = rat;
 		lat.setStatus(TrackerStatus.OK);
 		rat.setStatus(TrackerStatus.OK);
+		hipsWidth = server.config.getFloat("body.hipsWidth", hipsWidth);
+		hipsLength = server.config.getFloat("body.hipsLength", hipsLength);
+		ankleLength = server.config.getFloat("body.ankleLength", ankleLength);
 		
 		waistNode.attachChild(leftHipNode);
 		leftHipNode.localTransform.setTranslation(hipsWidth / 2, 0, 0);
@@ -63,16 +66,45 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 		rightHipNode.localTransform.setTranslation(-hipsWidth / 2, 0, 0);
 		
 		leftHipNode.attachChild(leftKneeNode);
-		leftKneeNode.localTransform.setTranslation(0, -hipLength, 0);
+		leftKneeNode.localTransform.setTranslation(0, -hipsLength, 0);
 		
 		rightHipNode.attachChild(rightKneeNode);
-		rightKneeNode.localTransform.setTranslation(0, -hipLength, 0);
+		rightKneeNode.localTransform.setTranslation(0, -hipsLength, 0);
 		
 		leftKneeNode.attachChild(leftAnkleNode);
 		leftAnkleNode.localTransform.setTranslation(0, -ankleLength, 0);
 		
 		rightKneeNode.attachChild(rightAnkleNode);
 		rightAnkleNode.localTransform.setTranslation(0, -ankleLength, 0);
+		
+		jointsMap.put(HumanJoint.HIPS_WIDTH, hipsWidth);
+		jointsMap.put(HumanJoint.HIPS_LENGTH, hipsLength);
+		jointsMap.put(HumanJoint.LEGS_LENGTH, ankleLength);
+	}
+	
+	@Override
+	public void sentJointLength(HumanJoint joint, float newLength) {
+		super.sentJointLength(joint, newLength);
+		switch(joint) {
+		case HIPS_WIDTH:
+			hipsWidth = newLength;
+			server.config.setProperty("body.hipsWidth", hipsWidth);
+			leftHipNode.localTransform.setTranslation(hipsWidth / 2, 0, 0);
+			rightHipNode.localTransform.setTranslation(-hipsWidth / 2, 0, 0);
+			break;
+		case HIPS_LENGTH:
+			hipsLength = newLength;
+			server.config.setProperty("body.hipsLength", hipsLength);
+			leftKneeNode.localTransform.setTranslation(0, -hipsLength, 0);
+			rightKneeNode.localTransform.setTranslation(0, -hipsLength, 0);
+			break;
+		case LEGS_LENGTH:
+			ankleLength = newLength;
+			server.config.setProperty("body.ankleLength", ankleLength);
+			leftAnkleNode.localTransform.setTranslation(0, -ankleLength, 0);
+			rightAnkleNode.localTransform.setTranslation(0, -ankleLength, 0);
+			break;
+		}
 	}
 	
 	@Override

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

@@ -1,10 +1,13 @@
 package io.eiren.vr.processor;
 
+import java.util.HashMap;
 import java.util.List;
+import java.util.Map;
 
 import com.jme3.math.Quaternion;
 import com.jme3.math.Vector3f;
 
+import io.eiren.util.ann.VRServerThread;
 import io.eiren.vr.VRServer;
 import io.eiren.vr.trackers.HMDTracker;
 import io.eiren.vr.trackers.Tracker;
@@ -12,6 +15,9 @@ import io.eiren.vr.trackers.TrackerStatus;
 
 public class HumanSkeleonWithWaist extends HumanSkeleton {
 	
+	protected final Map<HumanJoint, Float> jointsMap = new HashMap<>();
+	protected final VRServer server;
+	
 	protected final Quaternion qBuf = new Quaternion();
 	protected final Vector3f vBuf = new Vector3f();
 	
@@ -27,13 +33,13 @@ public class HumanSkeleonWithWaist extends HumanSkeleton {
 	/**
 	 * Distance from eyes to waist
 	 */
-	protected float waistDistance = 0.72f;
+	protected float waistDistance = 0.55f;
 	/**
 	 * Distance from eyes to waist, defines reported
 	 * tracker position, if you want to move resulting
 	 * tracker up or down from actual waist
 	 */
-	protected float trackerWaistDistance = 0.65f;
+	protected float trackerWaistDistance = 0.55f;
 	/**
 	 * Distacne from eyes to the base of the neck
 	 */
@@ -41,11 +47,12 @@ public class HumanSkeleonWithWaist extends HumanSkeleton {
 	/**
 	 * Distance from eyes to ear
 	 */
-	protected float headShift = 0.09f;
+	protected float headShift = 0.00f;
 
 	public HumanSkeleonWithWaist(VRServer server, Tracker waistTracker, List<ComputedHumanPoseTracker> computedTrackers) {
 		this.wasitTracker = waistTracker;
 		this.hmdTracker = server.hmdTracker;
+		this.server = server;
 		ComputedHumanPoseTracker cwt = null;
 		for(int i = 0; i < computedTrackers.size(); ++i) {
 			ComputedHumanPoseTracker t = computedTrackers.get(i);
@@ -54,7 +61,10 @@ public class HumanSkeleonWithWaist extends HumanSkeleton {
 		}
 		computedWaistTracker = cwt;
 		cwt.setStatus(TrackerStatus.OK);
+		headShift = server.config.getFloat("body.headShift", headShift);
+		neckLength = server.config.getFloat("body.neckLength", neckLength);
 		waistDistance = server.config.getFloat("body.waistDistance", waistDistance);
+		trackerWaistDistance = server.config.getFloat("body.trackerWaistDistance", trackerWaistDistance);
 		// Build skeleton
 		hmdNode.attachChild(headNode);
 		headNode.localTransform.setTranslation(0, 0, headShift);
@@ -63,10 +73,47 @@ public class HumanSkeleonWithWaist extends HumanSkeleton {
 		neckNode.localTransform.setTranslation(0, -neckLength, 0);
 		
 		neckNode.attachChild(waistNode);
-		waistNode.localTransform.setTranslation(0, -waistDistance + neckLength, 0);
+		waistNode.localTransform.setTranslation(0, -waistDistance, 0);
 		
 		neckNode.attachChild(trackerWaistNode);
-		trackerWaistNode.localTransform.setTranslation(0, -trackerWaistDistance + neckLength, 0);
+		trackerWaistNode.localTransform.setTranslation(0, -trackerWaistDistance, 0);
+		
+		jointsMap.put(HumanJoint.HEAD, headShift);
+		jointsMap.put(HumanJoint.NECK, neckLength);
+		jointsMap.put(HumanJoint.WAIST, waistDistance);
+		jointsMap.put(HumanJoint.WASIT_VIRTUAL, trackerWaistDistance);
+	}
+	
+	@Override
+	public Map<HumanJoint, Float> getJointsMap() {
+		return jointsMap;
+	}
+
+	@Override
+	public void sentJointLength(HumanJoint joint, float newLength) {
+		jointsMap.put(joint, newLength);
+		switch(joint) {
+		case HEAD:
+			headShift = newLength;
+			server.config.setProperty("body.headShift", headShift);
+			headNode.localTransform.setTranslation(0, 0, headShift);
+			break;
+		case NECK:
+			neckLength = newLength;
+			server.config.setProperty("body.neckLength", neckLength);
+			neckNode.localTransform.setTranslation(0, -neckLength, 0);
+			break;
+		case WAIST:
+			waistDistance = newLength;
+			server.config.setProperty("body.waistDistance", waistDistance);
+			waistNode.localTransform.setTranslation(0, -waistDistance, 0);
+			break;
+		case WASIT_VIRTUAL:
+			trackerWaistDistance = newLength;
+			server.config.setProperty("body.trackerWaistDistance", trackerWaistDistance);
+			trackerWaistNode.localTransform.setTranslation(0, -trackerWaistDistance, 0);
+			break;
+		}
 	}
 	
 	@Override
@@ -75,6 +122,7 @@ public class HumanSkeleonWithWaist extends HumanSkeleton {
 	}
 	
 	@Override
+	@VRServerThread
 	public void updatePose() {
 		updateLocalTransforms();
 		hmdNode.update();
@@ -91,8 +139,15 @@ public class HumanSkeleonWithWaist extends HumanSkeleton {
 		wasitTracker.getRotation(qBuf);
 		
 		neckNode.localTransform.setRotation(qBuf);
-		waistNode.localTransform.setRotation(qBuf);
 		trackerWaistNode.localTransform.setRotation(qBuf);
+
+		// Pelvic bone doesn't tilt when humans tilt, unless they really try.
+		// Can't calculate tilt without additional sensors, so just remove it
+		// completely.
+		qBuf.multLocal(vBuf);
+		vBuf.multLocal(1, 0, 1); // Keep only yaw / Don't normalize, it's done by lookAt()
+		qBuf.lookAt(vBuf, Vector3f.UNIT_Y);
+		waistNode.localTransform.setRotation(qBuf);
 	}
 	
 	protected void updateComputedTrackers() {

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

@@ -1,8 +1,41 @@
 package io.eiren.vr.processor;
 
+import java.util.Map;
+
+import io.eiren.util.ann.ThreadSafe;
+import io.eiren.util.ann.VRServerThread;
+
 public abstract class HumanSkeleton {
-	
+
+	@VRServerThread
 	public abstract void updatePose();
 	
+	@ThreadSafe
 	public abstract TransformNode getRootNode();
+
+	@ThreadSafe
+	public abstract Map<HumanJoint, Float> getJointsMap();
+
+	@ThreadSafe
+	public abstract void sentJointLength(HumanJoint joint, float newLength);
+	
+	public enum HumanJoint {
+		
+		HEAD("Head", ""),
+		NECK("Neck", ""),
+		WAIST("Waist", ""),
+		WASIT_VIRTUAL("Virtual waist", ""),
+		HIPS_WIDTH("Hips width", ""),
+		HIPS_LENGTH("Hips length", ""),
+		LEGS_LENGTH("Legs length", ""),
+		;
+		
+		public final String name;
+		public final String description;
+		
+		private HumanJoint(String name, String description) {
+			this.name = name;
+			this.description = description;
+		}
+	}
 }

src/main/java/io/eiren/vr/trackers/AdjustedTracker.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;
 
@@ -7,6 +8,10 @@ public class AdjustedTracker implements Tracker {
 	
 	public final Tracker tracker;
 	public final Quaternion adjustment = new Quaternion();
+	private final Quaternion smoothedQuaternion = new Quaternion();
+	private float[] angles = new float[3];
+	private float[] lastAngles = new float[3];
+	public float smooth = 2 * FastMath.DEG_TO_RAD;
 	
 	public AdjustedTracker(Tracker tracker) {
 		this.tracker = tracker;
@@ -26,6 +31,14 @@ public class AdjustedTracker implements Tracker {
 	@Override
 	public boolean getRotation(Quaternion store) {
 		tracker.getRotation(store);
+		store.toAngles(angles);
+		if(Math.abs(angles[0] - lastAngles[0]) > smooth || Math.abs(angles[1] - lastAngles[1]) > smooth || Math.abs(angles[2] - lastAngles[2]) > smooth) {
+			smoothedQuaternion.set(store);
+			store.toAngles(lastAngles);
+		} else {
+			store.set(smoothedQuaternion);
+		}
+		
 		adjustment.mult(store, store);
 		return true;
 	}

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

@@ -1,6 +1,12 @@
 package io.eiren.vr.trackers;
 
+import java.util.function.Consumer;
+
 public interface CalibratingTracker {
 	
-	public void startCalibration();
+	public void startCalibration(Consumer<String> calibrationDataConsumer);
+	
+	public void requestCalibrationData(Consumer<String> calibrationDataConsumer);
+	
+	public void uploadNewClibrationData();
 }

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

@@ -1,5 +1,8 @@
 package io.eiren.vr.trackers;
 
+import java.nio.ByteBuffer;
+import java.util.function.Consumer;
+
 import com.jme3.math.Quaternion;
 import com.jme3.math.Vector3f;
 
@@ -17,6 +20,7 @@ public class IMUTracker implements Tracker, CalibratingTracker {
 	protected final TrackersUDPServer server;
 	
 	protected BufferedTimer timer = new BufferedTimer(1f);
+	public CalibrationData newCalibrationData;
 	
 	public IMUTracker(String name, TrackersUDPServer server) {
 		this.name = name;
@@ -50,8 +54,8 @@ public class IMUTracker implements Tracker, CalibratingTracker {
 	}
 	
 	@Override
-	public void startCalibration() {
-		server.sendCalibrationCommand(this);
+	public void startCalibration(Consumer<String> calibrationDataConsumer) {
+		server.sendCalibrationCommand(this, calibrationDataConsumer);
 	}
 	
 	public float getTPS() {
@@ -61,4 +65,110 @@ public class IMUTracker implements Tracker, CalibratingTracker {
 	public void dataTick() {
 		timer.update();
 	}
+	
+	@Override
+	public void requestCalibrationData(Consumer<String> calibrationDataConsumer) {
+		server.requestCalibrationData(this, calibrationDataConsumer);
+	}
+	
+	@Override
+	public void uploadNewClibrationData() {
+		server.uploadNewCalibrationData(this, newCalibrationData);
+	}
+	
+	public static class CalibrationData {
+
+	    //acel offsets and correction matrix
+		float[] A_B = new float[3];
+	    float[][] A_Ainv = new float[3][3];
+	    // mag offsets and correction matrix
+	    float[] M_B = new float[3];
+	    float[][] M_Ainv = new float[3][3];
+	    //raw offsets, determined for gyro at rest
+	    float[] G_off = new float[3];
+	    
+	    public CalibrationData(double[] accelBasis, double[] accelAInv, double[] magBasis, double[] magAInv, double[] gyroOffset) {
+	    	A_B[0] = (float) accelBasis[0];
+	    	A_B[1] = (float) accelBasis[1];
+	    	A_B[2] = (float) accelBasis[2];
+	    	
+	    	A_Ainv[0][0] = (float) accelAInv[0];
+	    	A_Ainv[0][1] = (float) accelAInv[1];
+	    	A_Ainv[0][2] = (float) accelAInv[2];
+	    	A_Ainv[1][0] = (float) accelAInv[3];
+	    	A_Ainv[1][1] = (float) accelAInv[4];
+	    	A_Ainv[1][2] = (float) accelAInv[5];
+	    	A_Ainv[2][0] = (float) accelAInv[6];
+	    	A_Ainv[2][1] = (float) accelAInv[7];
+	    	A_Ainv[2][2] = (float) accelAInv[8];
+	    	
+	    	M_B[0] = (float) magBasis[0];
+	    	M_B[1] = (float) magBasis[1];
+	    	M_B[2] = (float) magBasis[2];
+	    	
+	    	M_Ainv[0][0] = (float) magAInv[0];
+	    	M_Ainv[0][1] = (float) magAInv[1];
+	    	M_Ainv[0][2] = (float) magAInv[2];
+	    	M_Ainv[1][0] = (float) magAInv[3];
+	    	M_Ainv[1][1] = (float) magAInv[4];
+	    	M_Ainv[1][2] = (float) magAInv[5];
+	    	M_Ainv[2][0] = (float) magAInv[6];
+	    	M_Ainv[2][1] = (float) magAInv[7];
+	    	M_Ainv[2][2] = (float) magAInv[8];
+	    	
+	    	G_off[0] = (float) gyroOffset[0];
+	    	G_off[1] = (float) gyroOffset[1];
+	    	G_off[2] = (float) gyroOffset[2];
+	    }
+		
+		public CalibrationData(ByteBuffer buffer) {
+			// Data is read in reverse, because it was reversed when sending
+			G_off[2] = buffer.getFloat();
+			G_off[1] = buffer.getFloat();
+			G_off[0] = buffer.getFloat();
+			
+			M_Ainv[2][2] = buffer.getFloat();
+			M_Ainv[2][1] = buffer.getFloat();
+			M_Ainv[2][0] = buffer.getFloat();
+			M_Ainv[1][2] = buffer.getFloat();
+			M_Ainv[1][1] = buffer.getFloat();
+			M_Ainv[1][0] = buffer.getFloat();
+			M_Ainv[0][2] = buffer.getFloat();
+			M_Ainv[0][1] = buffer.getFloat();
+			M_Ainv[0][0] = buffer.getFloat();
+			
+			M_B[2] = buffer.getFloat();
+			M_B[1] = buffer.getFloat();
+			M_B[0] = buffer.getFloat();
+			
+			A_Ainv[2][2] = buffer.getFloat();
+			A_Ainv[2][1] = buffer.getFloat();
+			A_Ainv[2][0] = buffer.getFloat();
+			A_Ainv[1][2] = buffer.getFloat();
+			A_Ainv[1][1] = buffer.getFloat();
+			A_Ainv[1][0] = buffer.getFloat();
+			A_Ainv[0][2] = buffer.getFloat();
+			A_Ainv[0][1] = buffer.getFloat();
+			A_Ainv[0][0] = buffer.getFloat();
+			
+			A_B[2] = buffer.getFloat();
+			A_B[1] = buffer.getFloat();
+			A_B[0] = buffer.getFloat();
+		}
+		
+		public String toTextMatrix() {
+			StringBuilder sb = new StringBuilder();
+			sb.append(String.format("{%8.2f,%8.2f,%8.2f},\n", A_B[0], A_B[1], A_B[2]));
+			sb.append(String.format("{{%9.5f,%9.5f,%9.5f},\n", A_Ainv[0][0], A_Ainv[0][1], A_Ainv[0][2]));
+			sb.append(String.format(" {%9.5f,%9.5f,%9.5f},\n", A_Ainv[1][0], A_Ainv[1][1], A_Ainv[1][2]));
+			sb.append(String.format(" {%9.5f,%9.5f,%9.5f}},\n", A_Ainv[2][0], A_Ainv[2][1], A_Ainv[2][2]));
+			sb.append(String.format("{%8.2f,%8.2f,%8.2f},\n", M_B[0], M_B[1], M_B[2]));
+			sb.append(String.format("{{%9.5f,%9.5f,%9.5f},\n", M_Ainv[0][0], M_Ainv[0][1], M_Ainv[0][2]));
+			sb.append(String.format(" {%9.5f,%9.5f,%9.5f},\n", M_Ainv[1][0], M_Ainv[1][1], M_Ainv[1][2]));
+			sb.append(String.format(" {%9.5f,%9.5f,%9.5f}},\n", M_Ainv[2][0], M_Ainv[2][1], M_Ainv[2][2]));
+			sb.append(String.format("{%8.2f, %8.2f, %8.2f}};\n", G_off[0], G_off[1], G_off[2]));
+			
+			return sb.toString();
+		}
+	}
 }

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

@@ -10,6 +10,7 @@ public class TrackerConfig {
 	public String designation;
 	public boolean hide;
 	public Quaternion adjustment;
+	public float trackerRotation = 0;
 	
 	public TrackerConfig(String trackerName) {
 		this.trackerName = trackerName;
@@ -19,6 +20,7 @@ public class TrackerConfig {
 		this.trackerName = node.getString("name");
 		this.designation = node.getString("designation");
 		this.hide = node.getBoolean("hide", false);
+		this.trackerRotation = node.getFloat("rotation", trackerRotation);
 		YamlNode adjNode = node.getNode("adjustment");
 		if(adjNode != null) {
 			adjustment = new Quaternion(adjNode.getFloat("x", 0), adjNode.getFloat("y", 0), adjNode.getFloat("z", 0), adjNode.getFloat("w", 0));
@@ -47,5 +49,10 @@ public class TrackerConfig {
 		} else {
 			configNode.removeProperty("adj");
 		}
+		if(trackerRotation != 0.0f) {
+			configNode.setProperty("rotation", trackerRotation);
+		} else {
+			configNode.removeProperty("rotation");
+		}
 	}
 }

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

@@ -20,6 +20,7 @@ import com.jme3.math.Vector3f;
 import io.eiren.hardware.magentometer.Magneto;
 import io.eiren.util.Util;
 import io.eiren.util.collections.FastList;
+import io.eiren.vr.trackers.IMUTracker.CalibrationData;
 
 /**
  * Recieves trackers data by UDP using extended owoTrack protocol.
@@ -34,11 +35,14 @@ 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];
+	private static final byte[] CALIBRATION_REQUEST_BUFFER = new byte[64];
 
 	private final Quaternion buf = new Quaternion();
 
 	private final List<TrackerConnection> trackers = new FastList<>();
 	private final Map<SocketAddress, TrackerConnection> trackersMap = new HashMap<>();
+	private final Map<Tracker, Consumer<String>> calibrationDataRequests = new HashMap<>();
+	private final Map<Tracker, Consumer<String>> newCalibrationDataRequests = new HashMap<>();
 	private final Consumer<IMUTracker> trackersConsumer;
 	private final int port;
 	
@@ -51,7 +55,7 @@ public class TrackersUDPServer extends Thread {
 		this.trackersConsumer = trackersConsumer;
 	}
 	
-	public void sendCalibrationCommand(Tracker tracker) {
+	public void sendCalibrationCommand(Tracker tracker, Consumer<String> calibrationDataConsumer) {
 		TrackerConnection connection = null;
 		synchronized(trackers) {
 			for(int i = 0; i < trackers.size(); ++i) {
@@ -70,6 +74,8 @@ public class TrackersUDPServer extends Thread {
 			connection.isCalibrating = true;
 			connection.rawCalibrationData.clear();
 		}
+		if(calibrationDataConsumer != null)
+			newCalibrationDataRequests.put(tracker, calibrationDataConsumer);
 		try {
 			socket.send(new DatagramPacket(CALIBRATION_BUFFER, CALIBRATION_BUFFER.length, connection.address));
 			System.out.println("[TrackerServer] Calibrating sensor on " + connection.address);
@@ -78,6 +84,48 @@ public class TrackersUDPServer extends Thread {
 		}
 	}
 	
+	public void requestCalibrationData(Tracker tracker, Consumer<String> consumer) {
+		TrackerConnection connection = null;
+		synchronized(trackers) {
+			for(int i = 0; i < trackers.size(); ++i) {
+				if(trackers.get(i).tracker == tracker) {
+					connection = trackers.get(i);
+					break;
+				}
+			}
+		}
+		if(connection == null)
+			return;
+		calibrationDataRequests.put(tracker, consumer);
+		try {
+			socket.send(new DatagramPacket(CALIBRATION_REQUEST_BUFFER, CALIBRATION_REQUEST_BUFFER.length, connection.address));
+			System.out.println("[TrackerServer] Requesting config from " + connection.address);
+		} catch(IOException e) {
+			e.printStackTrace();
+		}
+	}
+	
+	public void uploadNewCalibrationData(Tracker tracker, CalibrationData data) {
+		TrackerConnection connection = null;
+		synchronized(trackers) {
+			for(int i = 0; i < trackers.size(); ++i) {
+				if(trackers.get(i).tracker == tracker) {
+					connection = trackers.get(i);
+					break;
+				}
+			}
+		}
+		if(connection == null)
+			return;
+		// TODO
+		try {
+			socket.send(new DatagramPacket(CALIBRATION_REQUEST_BUFFER, CALIBRATION_REQUEST_BUFFER.length, connection.address));
+			System.out.println("[TrackerServer] Requesting config from " + connection.address);
+		} catch(IOException e) {
+			e.printStackTrace();
+		}
+	}
+	
 	private void stopCalibration(TrackerConnection sensor) {
 		synchronized(sensor) {
 			if(!sensor.isCalibrating)
@@ -117,8 +165,6 @@ public class TrackersUDPServer extends Thread {
 		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] =");
-		System.out.println(String.format("  {%8.2f, %8.2f, %8.2f}", gyroOffset[0], gyroOffset[1], gyroOffset[2]));
 		System.out.println("float A_B[3] =");
 		System.out.println(String.format("  {%8.2f,%8.2f,%8.2f},", accelBasis[0], accelBasis[1], accelBasis[2]));
 		System.out.println("float A_Ainv[3][3] =");
@@ -131,6 +177,16 @@ public class TrackersUDPServer extends Thread {
 		System.out.println(String.format("  {{%9.5f,%9.5f,%9.5f},", magAInv[0], magAInv[1], magAInv[2]));
 		System.out.println(String.format("  {%9.5f,%9.5f,%9.5f},", magAInv[3], magAInv[4], magAInv[5]));
 		System.out.println(String.format("  {%9.5f,%9.5f,%9.5f}},", magAInv[6], magAInv[7], magAInv[8]));
+		System.out.println("float G_off[3] =");
+		System.out.println(String.format("  {%8.2f, %8.2f, %8.2f}};", gyroOffset[0], gyroOffset[1], gyroOffset[2]));
+		
+		IMUTracker.CalibrationData data = new IMUTracker.CalibrationData(accelBasis, accelAInv, magBasis, magAInv, gyroOffset);
+		sensor.tracker.newCalibrationData = data;
+		
+		Consumer<String> consumer = newCalibrationDataRequests.remove(sensor.tracker);
+		if(consumer != null) {
+			consumer.accept(data.toTextMatrix());
+		}
 	}
 	
 	private void setUpNewSensor(DatagramPacket handshakePacket, ByteBuffer data) throws IOException {
@@ -158,7 +214,7 @@ public class TrackersUDPServer extends Thread {
 	
 	@Override
 	public void run() {
-		byte[] rcvBuffer = new byte[64];
+		byte[] rcvBuffer = new byte[512];
 		ByteBuffer bb = ByteBuffer.wrap(rcvBuffer).order(ByteOrder.BIG_ENDIAN);
 		try {
 			socket = new DatagramSocket(port);
@@ -220,6 +276,16 @@ public class TrackersUDPServer extends Thread {
 						bb.getLong();
 						sensor.gyroCalibrationData = new double[] {bb.getFloat(), bb.getFloat(), bb.getFloat()};
 						break;
+					case 8: // PACKET_CONFIG
+						if(sensor == null)
+							break;
+						bb.getLong();
+						IMUTracker.CalibrationData data = new IMUTracker.CalibrationData(bb);
+						Consumer<String> dataConsumer = calibrationDataRequests.remove(sensor.tracker);
+						if(dataConsumer != null) {
+							dataConsumer.accept(data.toTextMatrix());
+						}
+						break;
 					default:
 						System.out.println("[TrackerServer] Unknown data received: " + packetId + " from " + recieve.getSocketAddress());
 						break;
@@ -268,5 +334,7 @@ public class TrackersUDPServer extends Thread {
 		KEEPUP_BUFFER[3] = 1;
 		CALIBRATION_BUFFER[3] = 4;
 		CALIBRATION_BUFFER[4] = 1;
+		CALIBRATION_REQUEST_BUFFER[3] = 4;
+		CALIBRATION_REQUEST_BUFFER[4] = 2;
 	}
 }