diff --git a/README.md b/README.md index 4702f0fad2..d327656f76 100644 --- a/README.md +++ b/README.md @@ -45,3 +45,7 @@ run gradle command `shadowJar` to build a runnable server JAR * You must provide a copy of the original license (see LICENSE file) * You don't have to release your own software under MIT License or even open source at all, but you have to state that it's based on SlimeVR * This applies even if you distribute software without the source code + +## Contributions + +By contributing to this project you are placing all your code under MIT or less restricting licenses, and you certify that the code you have used is compatible with those licenses or is authored by you. If you're doing so on your work time, you certify that your employer is okay with this. diff --git a/slime-java-commons b/slime-java-commons index 35f5a78c20..cd6fd53324 160000 --- a/slime-java-commons +++ b/slime-java-commons @@ -1 +1 @@ -Subproject commit 35f5a78c20617a6769ce1e7f6fa9a6b8415d49b3 +Subproject commit cd6fd5332435de9303637be3a4dc47a2cecaecf1 diff --git a/src/main/java/dev/slimevr/Main.java b/src/main/java/dev/slimevr/Main.java index 9ba409b701..e43e40fdf7 100644 --- a/src/main/java/dev/slimevr/Main.java +++ b/src/main/java/dev/slimevr/Main.java @@ -15,7 +15,7 @@ public class Main { - public static String VERSION = "0.1.3"; + public static String VERSION = "0.1.4"; public static VRServer vrServer; diff --git a/src/main/java/dev/slimevr/NetworkProtocol.java b/src/main/java/dev/slimevr/NetworkProtocol.java new file mode 100644 index 0000000000..1300da1755 --- /dev/null +++ b/src/main/java/dev/slimevr/NetworkProtocol.java @@ -0,0 +1,9 @@ +package dev.slimevr; + +public enum NetworkProtocol { + + OWO_LEGACY, + SLIMEVR_RAW, + SLIMEVR_FLATBUFFER, + SLIMEVR_WEBSOCKET; +} diff --git a/src/main/java/dev/slimevr/VRServer.java b/src/main/java/dev/slimevr/VRServer.java index d9f5952ae4..61ceb9d70c 100644 --- a/src/main/java/dev/slimevr/VRServer.java +++ b/src/main/java/dev/slimevr/VRServer.java @@ -27,7 +27,7 @@ import dev.slimevr.vr.trackers.ShareableTracker; import dev.slimevr.vr.trackers.Tracker; import dev.slimevr.vr.trackers.TrackerConfig; -import dev.slimevr.vr.trackers.TrackersUDPServer; +import dev.slimevr.vr.trackers.udp.TrackersUDPServer; import io.eiren.util.OperatingSystem; import io.eiren.util.ann.ThreadSafe; import io.eiren.util.ann.ThreadSecure; diff --git a/src/main/java/dev/slimevr/gui/TrackersList.java b/src/main/java/dev/slimevr/gui/TrackersList.java index 9487ad57a0..fe43467a54 100644 --- a/src/main/java/dev/slimevr/gui/TrackersList.java +++ b/src/main/java/dev/slimevr/gui/TrackersList.java @@ -48,6 +48,7 @@ public class TrackersList extends EJBoxNoStretch { private final VRServer server; private final VRServerGUI gui; private long lastUpdate = 0; + private boolean debug = false; public TrackersList(VRServer server, VRServerGUI gui) { super(BoxLayout.PAGE_AXIS, false, true); @@ -59,6 +60,12 @@ public TrackersList(VRServer server, VRServerGUI gui) { server.addNewTrackerConsumer(this::newTrackerAdded); } + @AWTThread + public void setDebug(boolean debug) { + this.debug = debug; + build(); + } + @AWTThread private void build() { removeAll(); @@ -142,10 +149,12 @@ private class TrackerPanel extends EJBagNoStretch { JLabel magAccuracy; JLabel adj; JLabel adjYaw; + JLabel adjGyro; JLabel correction; JLabel signalStrength; JLabel rotQuat; JLabel rotAdj; + JLabel temperature; @AWTThread public TrackerPanel(Tracker t) { @@ -245,18 +254,17 @@ public void actionPerformed(ActionEvent e) { row++; add(new JLabel("Raw:"), c(0, row, 2, GridBagConstraints.FIRST_LINE_START)); add(raw = new JLabel("0 0 0"), s(c(1, row, 2, GridBagConstraints.FIRST_LINE_START), 3, 1)); - /* - if(realTracker instanceof IMUTracker) { + + if(debug && realTracker instanceof IMUTracker) { add(new JLabel("Quat:"), c(2, row, 2, GridBagConstraints.FIRST_LINE_START)); add(rotQuat = new JLabel("0"), c(3, row, 2, GridBagConstraints.FIRST_LINE_START)); } - //*/ row++; - /* - if(realTracker instanceof IMUTracker) { + + if(debug && realTracker instanceof IMUTracker) { add(new JLabel("Raw mag:"), c(0, row, 2, GridBagConstraints.FIRST_LINE_START)); add(rawMag = new JLabel("0 0 0"), s(c(1, row, 2, GridBagConstraints.FIRST_LINE_START), 3, 1)); - add(new JLabel("Hash:"), c(2, row, 2, GridBagConstraints.FIRST_LINE_START)); + add(new JLabel("Gyro fix:"), c(2, row, 2, GridBagConstraints.FIRST_LINE_START)); add(new JLabel(String.format("0x%8x", realTracker.hashCode())), s(c(3, row, 2, GridBagConstraints.FIRST_LINE_START), 3, 1)); row++; add(new JLabel("Cal:"), c(0, row, 2, GridBagConstraints.FIRST_LINE_START)); @@ -270,16 +278,18 @@ public void actionPerformed(ActionEvent e) { add(rotAdj = new JLabel("0"), c(3, row, 2, GridBagConstraints.FIRST_LINE_START)); row++; } - //*/ - /* - if(t instanceof ReferenceAdjustedTracker) { - add(new JLabel("Adj:"), c(0, row, 2, GridBagConstraints.FIRST_LINE_START)); + if(debug && t instanceof ReferenceAdjustedTracker) { + add(new JLabel("Att fix:"), c(0, row, 2, GridBagConstraints.FIRST_LINE_START)); add(adj = new JLabel("0 0 0 0"), c(1, row, 2, GridBagConstraints.FIRST_LINE_START)); - add(new JLabel("AdjY:"), c(2, row, 2, GridBagConstraints.FIRST_LINE_START)); + add(new JLabel("Yaw Fix:"), c(2, row, 2, GridBagConstraints.FIRST_LINE_START)); add(adjYaw = new JLabel("0 0 0 0"), c(3, row, 2, GridBagConstraints.FIRST_LINE_START)); + row++; + add(new JLabel("Gyro Fix:"), c(0, row, 2, GridBagConstraints.FIRST_LINE_START)); + add(adjGyro = new JLabel("0 0 0 0"), c(1, row, 2, GridBagConstraints.FIRST_LINE_START)); + add(new JLabel("Temp:"), c(2, row, 2, GridBagConstraints.FIRST_LINE_START)); + add(temperature = new JLabel("?"), c(3, row, 2, GridBagConstraints.FIRST_LINE_START)); } - //*/ setBorder(BorderFactory.createLineBorder(new Color(0x663399), 2, false)); TrackersList.this.add(this); @@ -314,21 +324,30 @@ public void update() { if(realTracker instanceof TrackerWithBattery) bat.setText(String.format("%d%% (%sV)", Math.round(((TrackerWithBattery) realTracker).getBatteryLevel()), StringUtils.prettyNumber(((TrackerWithBattery) realTracker).getBatteryVoltage(), 2))); if(t instanceof ReferenceAdjustedTracker) { - ((ReferenceAdjustedTracker) t).attachmentFix.toAngles(angles); - if(adj != null) + ReferenceAdjustedTracker rat = (ReferenceAdjustedTracker) t; + if(adj != null) { + rat.attachmentFix.toAngles(angles); adj.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)); - ((ReferenceAdjustedTracker) t).yawFix.toAngles(angles); - if(adjYaw != null) + } + if(adjYaw != null) { + rat.yawFix.toAngles(angles); adjYaw.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(adjGyro != null) { + rat.gyroFix.toAngles(angles); + adjGyro.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) { if(ping != null) ping.setText(String.valueOf(((IMUTracker) realTracker).ping)); - if (signalStrength != null) { + if(signalStrength != null) { int signal = ((IMUTracker) realTracker).signalStrength; if (signal == -1) { signalStrength.setText("N/A"); @@ -347,21 +366,23 @@ public void update() { + " " + StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0)); if(realTracker instanceof IMUTracker) { IMUTracker imu = (IMUTracker) realTracker; - imu.rotMagQuaternion.toAngles(angles); - if(rawMag != null) + if(rawMag != null) { + imu.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)); + } if(calibration != null) calibration.setText(imu.calibrationStatus + " / " + imu.magCalibrationStatus); if(magAccuracy != null) magAccuracy.setText(StringUtils.prettyNumber(imu.magnetometerAccuracy * FastMath.RAD_TO_DEG, 1) + "°"); - imu.getCorrection(q); - q.toAngles(angles); - if(correction != null) + if(correction != null) { + imu.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)); + } if(rotQuat != null) { imu.rotQuaternion.toAngles(angles); rotQuat.setText(StringUtils.prettyNumber(angles[0] * FastMath.RAD_TO_DEG, 0) @@ -374,6 +395,14 @@ public void update() { + " " + StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 0) + " " + StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0)); } + if(temperature != null) { + if(imu.temperature == 0.0f) { + // Can't be exact 0, so no info received + temperature.setText("?"); + } else { + temperature.setText(StringUtils.prettyNumber(imu.temperature, 1) + "∘C"); + } + } } } } diff --git a/src/main/java/dev/slimevr/gui/VRServerGUI.java b/src/main/java/dev/slimevr/gui/VRServerGUI.java index e43b90e0d8..01d639c964 100644 --- a/src/main/java/dev/slimevr/gui/VRServerGUI.java +++ b/src/main/java/dev/slimevr/gui/VRServerGUI.java @@ -198,6 +198,17 @@ public void mouseClicked(MouseEvent e) { add(new EJBoxNoStretch(PAGE_AXIS, false, true) {{ setAlignmentY(TOP_ALIGNMENT); + + JCheckBox debugCb; + add(debugCb = new JCheckBox("Show debug information")); + debugCb.setSelected(false); + debugCb.addActionListener(new ActionListener() { + @Override + public void actionPerformed(ActionEvent e) { + trackersList.setDebug(debugCb.isSelected()); + } + }); + JLabel l; add(l = new JLabel("Body proportions")); l.setFont(l.getFont().deriveFont(Font.BOLD)); diff --git a/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java b/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java index e917186c9a..449295cddb 100644 --- a/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java +++ b/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java @@ -4,6 +4,7 @@ import com.jme3.math.Quaternion; import com.jme3.math.Vector3f; +import dev.slimevr.vr.trackers.udp.TrackersUDPServer; import io.eiren.util.BufferedTimer; public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery { @@ -36,9 +37,8 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery { protected BufferedTimer timer = new BufferedTimer(1f); public int ping = -1; public int signalStrength = -1; + public float temperature = 0; - public StringBuilder serialBuffer = new StringBuilder(); - long lastSerialUpdate = 0; public TrackerPosition bodyPosition = null; public IMUTracker(int trackerId, String name, String descriptiveName, TrackersUDPServer server) { diff --git a/src/main/java/dev/slimevr/vr/trackers/MPUTracker.java b/src/main/java/dev/slimevr/vr/trackers/MPUTracker.java index 37e9df888b..a4a553fd10 100644 --- a/src/main/java/dev/slimevr/vr/trackers/MPUTracker.java +++ b/src/main/java/dev/slimevr/vr/trackers/MPUTracker.java @@ -2,6 +2,8 @@ import java.nio.ByteBuffer; +import dev.slimevr.vr.trackers.udp.TrackersUDPServer; + public class MPUTracker extends IMUTracker { public ConfigurationData newCalibrationData; diff --git a/src/main/java/dev/slimevr/vr/trackers/BnoTap.java b/src/main/java/dev/slimevr/vr/trackers/SensorTap.java similarity index 81% rename from src/main/java/dev/slimevr/vr/trackers/BnoTap.java rename to src/main/java/dev/slimevr/vr/trackers/SensorTap.java index 4a37f6922e..12ae3413d9 100644 --- a/src/main/java/dev/slimevr/vr/trackers/BnoTap.java +++ b/src/main/java/dev/slimevr/vr/trackers/SensorTap.java @@ -1,10 +1,10 @@ package dev.slimevr.vr.trackers; -public class BnoTap { +public class SensorTap { public final boolean doubleTap; - public BnoTap(int tapBits) { + public SensorTap(int tapBits) { doubleTap = (tapBits & 0x40) > 0; } diff --git a/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java b/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java deleted file mode 100644 index 318ed24582..0000000000 --- a/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java +++ /dev/null @@ -1,488 +0,0 @@ -package dev.slimevr.vr.trackers; - -import java.io.IOException; -import java.io.UnsupportedEncodingException; -import java.net.DatagramPacket; -import java.net.DatagramSocket; -import java.net.Inet6Address; -import java.net.InetAddress; -import java.net.InetSocketAddress; -import java.net.NetworkInterface; -import java.net.SocketAddress; -import java.net.SocketTimeoutException; -import java.nio.ByteBuffer; -import java.nio.ByteOrder; -import java.util.ArrayList; -import java.util.Enumeration; -import java.util.HashMap; -import java.util.Iterator; -import java.util.List; -import java.util.Map; -import java.util.Random; -import java.util.function.Consumer; - -import com.jme3.math.FastMath; -import com.jme3.math.Quaternion; -import com.jme3.math.Vector3f; - -import io.eiren.util.Util; -import io.eiren.util.collections.FastList; - -/** - * Recieves trackers data by UDP using extended owoTrack protocol. - */ -public class TrackersUDPServer extends Thread { - - enum ProtocolCompat { - OWOTRACK_LEGACY, - OWOTRACK8, - SLIMEVR - } - - /** - * Change between IMU axises and OpenGL/SteamVR axises - */ - private static final Quaternion offset = new Quaternion().fromAngleAxis(-FastMath.HALF_PI, Vector3f.UNIT_X); - - private static final byte[] HANDSHAKE_BUFFER = new byte[64]; - private static final byte[] KEEPUP_BUFFER = new byte[64]; - private static final byte[] CALIBRATION_BUFFER = new byte[64]; - private static final byte[] CALIBRATION_REQUEST_BUFFER = new byte[64]; - - private final Quaternion buf = new Quaternion(); - private final Random random = new Random(); - private final List trackers = new FastList<>(); - private final Map trackersMap = new HashMap<>(); - private final Map> calibrationDataRequests = new HashMap<>(); - private final Consumer trackersConsumer; - private final int port; - - protected DatagramSocket socket = null; - protected long lastKeepup = System.currentTimeMillis(); - - public TrackersUDPServer(int port, String name, Consumer trackersConsumer) { - super(name); - this.port = port; - this.trackersConsumer = trackersConsumer; - } - - private void setUpNewSensor(DatagramPacket handshakePacket, ByteBuffer data) throws IOException { - System.out.println("[TrackerServer] Handshake recieved from " + handshakePacket.getAddress() + ":" + handshakePacket.getPort()); - InetAddress addr = handshakePacket.getAddress(); - TrackerConnection sensor; - synchronized(trackers) { - sensor = trackersMap.get(addr); - } - if(sensor == null) { - data.getLong(); // Skip packet number - int boardType = -1; - int imuType = -1; - int firmwareBuild = -1; - 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(); - data.getInt(); - } - if(data.remaining() > 3) - firmwareBuild = data.getInt(); - 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; - 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(macString.equals("00:00:00:00:00:00")) - macString = null; - } - } - ProtocolCompat protocolCompat = ProtocolCompat.SLIMEVR; - if(firmware.length() == 0) { - firmware.append("owoTrack"); - protocolCompat = ProtocolCompat.OWOTRACK_LEGACY; - } else if (firmware.toString().equals("owoTrack8")) { - protocolCompat = ProtocolCompat.OWOTRACK8; - } - String trackerName = macString != null ? "udp://" + macString : "udp:/" + handshakePacket.getAddress().toString(); - String descriptiveName = "udp:/" + handshakePacket.getAddress().toString(); - IMUTracker imu = new IMUTracker(Tracker.getNextLocalTrackerId(), trackerName, descriptiveName, this); - ReferenceAdjustedTracker adjustedTracker = new ReferenceAdjustedTracker<>(imu); - trackersConsumer.accept(adjustedTracker); - sensor = new TrackerConnection(imu, handshakePacket.getSocketAddress(), protocolCompat); - int i = 0; - synchronized(trackers) { - i = trackers.size(); - trackers.add(sensor); - trackersMap.put(addr, sensor); - } - System.out.println("[TrackerServer] Sensor " + i + " added with address " + handshakePacket.getSocketAddress() + ". Board type: " + boardType + ", imu type: " + imuType + ", firmware: " + firmware + " (" + firmwareBuild + "), mac: " + macString + ", name: " + trackerName); - } - sensor.sensors.get(0).setStatus(TrackerStatus.OK); - socket.send(new DatagramPacket(HANDSHAKE_BUFFER, HANDSHAKE_BUFFER.length, handshakePacket.getAddress(), handshakePacket.getPort())); - } - - private void setUpAuxilarySensor(TrackerConnection connection, int trackerId) throws IOException { - System.out.println("[TrackerServer] Setting up auxilary sensor for " + connection.sensors.get(0).getName()); - IMUTracker imu = connection.sensors.get(trackerId); - if(imu == null) { - imu = new IMUTracker(Tracker.getNextLocalTrackerId(), connection.sensors.get(0).getName() + "/" + trackerId, connection.sensors.get(0).getDescriptiveName() + "/" + trackerId, this); - connection.sensors.put(trackerId, imu); - ReferenceAdjustedTracker adjustedTracker = new ReferenceAdjustedTracker<>(imu); - trackersConsumer.accept(adjustedTracker); - System.out.println("[TrackerServer] Sensor added with address " + imu.getName()); - } - imu.setStatus(TrackerStatus.OK); - } - - - @Override - public void run() { - byte[] rcvBuffer = new byte[512]; - ByteBuffer bb = ByteBuffer.wrap(rcvBuffer).order(ByteOrder.BIG_ENDIAN); - StringBuilder serialBuffer2 = new StringBuilder(); - try { - socket = new DatagramSocket(port); - - // Why not just 255.255.255.255? Because Windows. - // https://social.technet.microsoft.com/Forums/windows/en-US/72e7387a-9f2c-4bf4-a004-c89ddde1c8aa/how-to-fix-the-global-broadcast-address-255255255255-behavior-on-windows - ArrayList addresses = new ArrayList(); - Enumeration ifaces = NetworkInterface.getNetworkInterfaces(); - while (ifaces.hasMoreElements()) { - NetworkInterface iface = ifaces.nextElement(); - // Ignore loopback, PPP, virtual and disabled devices - if (iface.isLoopback() || !iface.isUp() || iface.isPointToPoint() || iface.isVirtual()) { - continue; - } - Enumeration iaddrs = iface.getInetAddresses(); - while (iaddrs.hasMoreElements()) { - InetAddress iaddr = iaddrs.nextElement(); - // Ignore IPv6 addresses - if (iaddr instanceof Inet6Address) { - continue; - } - String[] iaddrParts = iaddr.getHostAddress().split("\\."); - addresses.add(new InetSocketAddress(String.format("%s.%s.%s.255", iaddrParts[0], iaddrParts[1], iaddrParts[2]), port)); - } - } - byte[] dummyPacket = new byte[] {0x0}; - - long prevPacketTime = System.currentTimeMillis(); - socket.setSoTimeout(250); - while(true) { - try { - boolean hasActiveTrackers = false; - for (TrackerConnection tracker: trackers) { - if (tracker.sensors.get(0).getStatus() == TrackerStatus.OK) { - hasActiveTrackers = true; - break; - } - } - if (!hasActiveTrackers) { - long discoveryPacketTime = System.currentTimeMillis(); - if ((discoveryPacketTime - prevPacketTime) >= 2000) { - for (SocketAddress addr: addresses) { - socket.send(new DatagramPacket(dummyPacket, dummyPacket.length, addr)); - } - prevPacketTime = discoveryPacketTime; - } - } - - DatagramPacket recieve = new DatagramPacket(rcvBuffer, rcvBuffer.length); - socket.receive(recieve); - bb.rewind(); - - TrackerConnection connection; - IMUTracker tracker = null; - synchronized(trackers) { - connection = trackersMap.get(recieve.getAddress()); - } - if(connection != null) - connection.lastPacket = System.currentTimeMillis(); - int packetId; - switch(packetId = bb.getInt()) { - case 0: - break; - case 3: - setUpNewSensor(recieve, bb); - break; - case 1: // PACKET_ROTATION - case 16: // PACKET_ROTATION_2 - if(connection == null) - break; - bb.getLong(); - buf.set(bb.getFloat(), bb.getFloat(), bb.getFloat(), bb.getFloat()); - offset.mult(buf, buf); - if(packetId == 1) { - tracker = connection.sensors.get(0); - } else { - tracker = connection.sensors.get(1); - } - if(tracker == null) - break; - tracker.rotQuaternion.set(buf); - tracker.dataTick(); - break; - case 17: // PACKET_ROTATION_DATA - if(connection == null) - break; - if(connection.protocol == ProtocolCompat.OWOTRACK_LEGACY) - break; - bb.getLong(); - int sensorId = bb.get() & 0xFF; - tracker = connection.sensors.get(sensorId); - 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; - if(connection.protocol == ProtocolCompat.OWOTRACK_LEGACY) - break; - bb.getLong(); - sensorId = bb.get() & 0xFF; - tracker = connection.sensors.get(sensorId); - if(tracker == null) - break; - float accuracyInfo = bb.getFloat(); - tracker.magnetometerAccuracy = accuracyInfo; - break; - case 2: // PACKET_GYRO - case 4: // PACKET_ACCEL - case 5: // PACKET_MAG - case 9: // PACKET_RAW_MAGENTOMETER - break; // None of these packets are used by SlimeVR trackers and are deprecated, use more generic PACKET_ROTATION_DATA - case 8: // PACKET_CONFIG - if(connection == null) - break; - if(connection.protocol == ProtocolCompat.OWOTRACK_LEGACY) - break; - bb.getLong(); - MPUTracker.ConfigurationData data = new MPUTracker.ConfigurationData(bb); - Consumer dataConsumer = calibrationDataRequests.remove(connection.sensors.get(0)); - if(dataConsumer != null) { - dataConsumer.accept(data.toTextMatrix()); - } - break; - case 10: // PACKET_PING_PONG: - if(connection == null) - break; - if(connection.protocol == ProtocolCompat.OWOTRACK_LEGACY) - break; - int pingId = bb.getInt(); - if(connection.lastPingPacketId == pingId) { - for(int i = 0; i < connection.sensors.size(); ++i) { - tracker = connection.sensors.get(i); - tracker.ping = (int) (System.currentTimeMillis() - connection.lastPingPacketTime) / 2; - tracker.dataTick(); - } - } - break; - case 11: // PACKET_SERIAL - if(connection == null) - break; - if(connection.protocol == ProtocolCompat.OWOTRACK_LEGACY) - break; - tracker = connection.sensors.get(0); - bb.getLong(); - int length = bb.getInt(); - for(int i = 0; i < length; ++i) { - char ch = (char) bb.get(); - if(ch == '\n') { - serialBuffer2.append('[').append(tracker.getName()).append("] ").append(tracker.serialBuffer); - System.out.println(serialBuffer2.toString()); - serialBuffer2.setLength(0); - tracker.serialBuffer.setLength(0); - } else { - tracker.serialBuffer.append(ch); - } - } - break; - case 12: // PACKET_BATTERY_VOLTAGE - if(connection == null) - break; - tracker = connection.sensors.get(0); - bb.getLong(); - if(connection.protocol == ProtocolCompat.OWOTRACK_LEGACY || connection.protocol == ProtocolCompat.OWOTRACK8) { - tracker.setBatteryLevel(bb.getFloat()); - break; - } - tracker.setBatteryVoltage(bb.getFloat()); - tracker.setBatteryLevel(bb.getFloat() * 100); - break; - case 13: // PACKET_TAP - if(connection == null) - break; - if(connection.protocol == ProtocolCompat.OWOTRACK_LEGACY) - break; - bb.getLong(); - sensorId = bb.get() & 0xFF; - tracker = connection.sensors.get(sensorId); - if(tracker == null) - break; - 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(connection == null) - break; - sensorId = bb.get() & 0xFF; - tracker = connection.sensors.get(sensorId); - if(tracker == null) - break; - tracker.setStatus(TrackerStatus.ERROR); - break; - case 15: // PACKET_SENSOR_INFO - if(connection == null) - break; - bb.getLong(); - sensorId = bb.get() & 0xFF; - int sensorStatus = bb.get() & 0xFF; - if(sensorId > 0 && sensorStatus == 1) { - setUpAuxilarySensor(connection, sensorId); - } - bb.rewind(); - bb.putInt(15); - bb.put((byte) sensorId); - bb.put((byte) sensorStatus); - socket.send(new DatagramPacket(rcvBuffer, bb.position(), connection.address)); - System.out.println("[TrackerServer] Sensor info for " + connection.sensors.get(0).getName() + "/" + sensorId + ": " + sensorStatus); - break; - case 19: - if(connection == null) - break; - bb.getLong(); - sensorId = bb.get() & 0xFF; - tracker = connection.sensors.get(sensorId); - if(tracker == null) - break; - int signalStrength = bb.get(); - tracker.signalStrength = signalStrength; - break; - default: - System.out.println("[TrackerServer] Unknown data received: " + packetId + " from " + recieve.getSocketAddress()); - break; - } - } catch(SocketTimeoutException e) { - } catch(Exception e) { - e.printStackTrace(); - } - if(lastKeepup + 500 < System.currentTimeMillis()) { - lastKeepup = System.currentTimeMillis(); - synchronized(trackers) { - for(int i = 0; i < trackers.size(); ++i) { - TrackerConnection conn = trackers.get(i); - socket.send(new DatagramPacket(KEEPUP_BUFFER, KEEPUP_BUFFER.length, conn.address)); - if(conn.lastPacket + 1000 < System.currentTimeMillis()) { - Iterator iterator = conn.sensors.values().iterator(); - while(iterator.hasNext()) { - IMUTracker tracker = iterator.next(); - if(tracker.getStatus() == TrackerStatus.OK) - tracker.setStatus(TrackerStatus.DISCONNECTED); - } - } else { - Iterator iterator = conn.sensors.values().iterator(); - while(iterator.hasNext()) { - IMUTracker tracker = iterator.next(); - if(tracker.getStatus() == TrackerStatus.DISCONNECTED) - tracker.setStatus(TrackerStatus.OK); - } - } - IMUTracker tracker = conn.sensors.get(0); - if(tracker == null) - continue; - if(tracker.serialBuffer.length() > 0) { - if(tracker.lastSerialUpdate + 500L < System.currentTimeMillis()) { - serialBuffer2.append('[').append(tracker.getName()).append("] ").append(tracker.serialBuffer); - System.out.println(serialBuffer2.toString()); - serialBuffer2.setLength(0); - tracker.serialBuffer.setLength(0); - } - } - if(conn.lastPingPacketTime + 500 < System.currentTimeMillis()) { - conn.lastPingPacketId = random.nextInt(); - conn.lastPingPacketTime = System.currentTimeMillis(); - bb.rewind(); - bb.putInt(10); - bb.putInt(conn.lastPingPacketId); - socket.send(new DatagramPacket(rcvBuffer, bb.position(), conn.address)); - } - } - } - } - } - } catch(Exception e) { - e.printStackTrace(); - } finally { - Util.close(socket); - } - } - - private class TrackerConnection { - - Map sensors = new HashMap<>(); - SocketAddress address; - public long lastPacket = System.currentTimeMillis(); - public int lastPingPacketId = -1; - public long lastPingPacketTime = 0; - public ProtocolCompat protocol; - - public TrackerConnection(IMUTracker tracker, SocketAddress address, ProtocolCompat protocol) { - this.sensors.put(0, tracker); - this.address = address; - this.protocol = protocol; - } - } - - static { - try { - HANDSHAKE_BUFFER[0] = 3; - byte[] str = "Hey OVR =D 5".getBytes("ASCII"); - System.arraycopy(str, 0, HANDSHAKE_BUFFER, 1, str.length); - } catch(UnsupportedEncodingException e) { - throw new AssertionError(e); - } - KEEPUP_BUFFER[3] = 1; - CALIBRATION_BUFFER[3] = 4; - CALIBRATION_BUFFER[4] = 1; - CALIBRATION_REQUEST_BUFFER[3] = 4; - CALIBRATION_REQUEST_BUFFER[4] = 2; - } -} diff --git a/src/main/java/dev/slimevr/vr/trackers/udp/SensorSpecificPacket.java b/src/main/java/dev/slimevr/vr/trackers/udp/SensorSpecificPacket.java new file mode 100644 index 0000000000..accb17ecc0 --- /dev/null +++ b/src/main/java/dev/slimevr/vr/trackers/udp/SensorSpecificPacket.java @@ -0,0 +1,15 @@ +package dev.slimevr.vr.trackers.udp; + +public interface SensorSpecificPacket { + + public int getSensorId(); + + /** + * Sensor with id 255 is "global" representing a whole device + * @param sensorId + * @return + */ + public static boolean isGlobal(int sensorId) { + return sensorId == 255; + } +} diff --git a/src/main/java/dev/slimevr/vr/trackers/udp/TrackerUDPConnection.java b/src/main/java/dev/slimevr/vr/trackers/udp/TrackerUDPConnection.java new file mode 100644 index 0000000000..1936d2045b --- /dev/null +++ b/src/main/java/dev/slimevr/vr/trackers/udp/TrackerUDPConnection.java @@ -0,0 +1,43 @@ +package dev.slimevr.vr.trackers.udp; + +import java.net.InetAddress; +import java.net.SocketAddress; +import java.util.HashMap; +import java.util.Map; + +import dev.slimevr.NetworkProtocol; +import dev.slimevr.vr.trackers.IMUTracker; + +public class TrackerUDPConnection { + + public Map sensors = new HashMap<>(); + public SocketAddress address; + public InetAddress ipAddress; + public long lastPacket = System.currentTimeMillis(); + public int lastPingPacketId = -1; + public long lastPingPacketTime = 0; + public String name; + public String descriptiveName; + public StringBuilder serialBuffer = new StringBuilder(); + public long lastSerialUpdate = 0; + public long lastPacketNumber = -1; + public NetworkProtocol protocol = null; + public int firmwareBuild = 0; + + public TrackerUDPConnection(SocketAddress address, InetAddress ipAddress) { + this.address = address; + this.ipAddress = ipAddress; + } + + public boolean isNextPacket(long packetId) { + if(packetId != 0 && packetId <= lastPacketNumber) + return false; + lastPacketNumber = packetId; + return true; + } + + @Override + public String toString() { + return "udp:/" + ipAddress; + } +} \ No newline at end of file diff --git a/src/main/java/dev/slimevr/vr/trackers/udp/TrackersUDPServer.java b/src/main/java/dev/slimevr/vr/trackers/udp/TrackersUDPServer.java new file mode 100644 index 0000000000..7ce32afc82 --- /dev/null +++ b/src/main/java/dev/slimevr/vr/trackers/udp/TrackersUDPServer.java @@ -0,0 +1,429 @@ +package dev.slimevr.vr.trackers.udp; + +import java.io.IOException; +import java.net.DatagramPacket; +import java.net.DatagramSocket; +import java.net.Inet6Address; +import java.net.InetAddress; +import java.net.InetSocketAddress; +import java.net.NetworkInterface; +import java.net.SocketAddress; +import java.net.SocketTimeoutException; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import java.util.ArrayList; +import java.util.Collection; +import java.util.Enumeration; +import java.util.HashMap; +import java.util.Iterator; +import java.util.List; +import java.util.Map; +import java.util.Random; +import java.util.function.Consumer; + +import org.apache.commons.lang3.ArrayUtils; + +import com.jme3.math.FastMath; +import com.jme3.math.Quaternion; +import com.jme3.math.Vector3f; + +import dev.slimevr.NetworkProtocol; +import dev.slimevr.vr.trackers.IMUTracker; +import dev.slimevr.vr.trackers.ReferenceAdjustedTracker; +import dev.slimevr.vr.trackers.Tracker; +import dev.slimevr.vr.trackers.TrackerStatus; +import io.eiren.util.Util; +import io.eiren.util.collections.FastList; +import io.eiren.util.logging.LogManager; + +/** + * Recieves trackers data by UDP using extended owoTrack protocol. + */ +public class TrackersUDPServer extends Thread { + + /** + * Change between IMU axises and OpenGL/SteamVR axises + */ + private static final Quaternion offset = new Quaternion().fromAngleAxis(-FastMath.HALF_PI, Vector3f.UNIT_X); + + private final Quaternion buf = new Quaternion(); + private final Random random = new Random(); + private final List connections = new FastList<>(); + private final Map connectionsByAddress = new HashMap<>(); + private final Map connectionsByMAC = new HashMap<>(); + private final Consumer trackersConsumer; + private final int port; + private final ArrayList broadcastAddresses = new ArrayList<>(); + private final UDPProtocolParser parser = new UDPProtocolParser(); + private final byte[] rcvBuffer = new byte[512]; + private final ByteBuffer bb = ByteBuffer.wrap(rcvBuffer).order(ByteOrder.BIG_ENDIAN); + + protected DatagramSocket socket = null; + protected long lastKeepup = System.currentTimeMillis(); + + public TrackersUDPServer(int port, String name, Consumer trackersConsumer) { + super(name); + this.port = port; + this.trackersConsumer = trackersConsumer; + try { + Enumeration ifaces = NetworkInterface.getNetworkInterfaces(); + while(ifaces.hasMoreElements()) { + NetworkInterface iface = ifaces.nextElement(); + // Ignore loopback, PPP, virtual and disabled devices + if(iface.isLoopback() || !iface.isUp() || iface.isPointToPoint() || iface.isVirtual()) { + continue; + } + Enumeration iaddrs = iface.getInetAddresses(); + while(iaddrs.hasMoreElements()) { + InetAddress iaddr = iaddrs.nextElement(); + // Ignore IPv6 addresses + if(iaddr instanceof Inet6Address) { + continue; + } + String[] iaddrParts = iaddr.getHostAddress().split("\\."); + broadcastAddresses.add(new InetSocketAddress(String.format("%s.%s.%s.255", iaddrParts[0], iaddrParts[1], iaddrParts[2]), port)); + } + } + } catch(Exception e) { + LogManager.log.severe("[TrackerServer] Can't enumerate network interfaces", e); + } + } + + private void setUpNewConnection(DatagramPacket handshakePacket, UDPPacket3Handshake handshake) throws IOException { + LogManager.log.info("[TrackerServer] Handshake recieved from " + handshakePacket.getAddress() + ":" + handshakePacket.getPort()); + InetAddress addr = handshakePacket.getAddress(); + TrackerUDPConnection connection; + synchronized(connections) { + connection = connectionsByAddress.get(addr); + } + if(connection == null) { + connection = new TrackerUDPConnection(handshakePacket.getSocketAddress(), addr); + connection.firmwareBuild = handshake.firmwareBuild; + if(handshake.firmware == null || handshake.firmware.length() == 0) { + // Only old owoTrack doesn't report firmware and have differenet packet IDs with SlimeVR + connection.protocol = NetworkProtocol.OWO_LEGACY; + } else { + connection.protocol = NetworkProtocol.SLIMEVR_RAW; + } + connection.name = handshake.macString != null ? "udp://" + handshake.macString : "udp:/" + handshakePacket.getAddress().toString(); + connection.descriptiveName = "udp:/" + handshakePacket.getAddress().toString(); + int i = 0; + synchronized(connections) { + if(handshake.macString != null && connectionsByMAC.containsKey(handshake.macString)) { + TrackerUDPConnection previousConnection = connectionsByMAC.get(handshake.macString); + i = connections.indexOf(previousConnection); + connectionsByAddress.remove(previousConnection.ipAddress); + previousConnection.lastPacketNumber = 0; + previousConnection.ipAddress = addr; + previousConnection.address = handshakePacket.getSocketAddress(); + previousConnection.name = connection.name; + previousConnection.descriptiveName = connection.descriptiveName; + connectionsByAddress.put(addr, previousConnection); + LogManager.log.info("[TrackerServer] Tracker " + i + " handed over to address " + handshakePacket.getSocketAddress() + ". Board type: " + handshake.boardType + ", imu type: " + handshake.imuType + ", firmware: " + handshake.firmware + " (" + connection.firmwareBuild + "), mac: " + handshake.macString + ", name: " + previousConnection.name); + } else { + i = connections.size(); + connections.add(connection); + connectionsByAddress.put(addr, connection); + if(handshake.macString != null) { + connectionsByMAC.put(handshake.macString, connection); + } + LogManager.log.info("[TrackerServer] Tracker " + i + " added with address " + handshakePacket.getSocketAddress() + ". Board type: " + handshake.boardType + ", imu type: " + handshake.imuType + ", firmware: " + handshake.firmware + " (" + connection.firmwareBuild + "), mac: " + handshake.macString + ", name: " + connection.name); + } + } + if(connection.protocol == NetworkProtocol.OWO_LEGACY || connection.firmwareBuild < 8) { + // Set up new sensor for older firmware + // Firmware after 7 should send sensor status packet and sensor will be created when it's received + setUpSensor(connection, 0, handshake.imuType, 1); + } + } + bb.limit(bb.capacity()); + bb.rewind(); + parser.writeHandshakeResponse(bb, connection); + socket.send(new DatagramPacket(rcvBuffer, bb.position(), connection.address)); + } + + private void setUpSensor(TrackerUDPConnection connection, int trackerId, int sensorType, int sensorStatus) throws IOException { + LogManager.log.info("[TrackerServer] Sensor " + trackerId + " for " + connection.name + " status: " + sensorStatus); + IMUTracker imu = connection.sensors.get(trackerId); + if(imu == null) { + imu = new IMUTracker(Tracker.getNextLocalTrackerId(), connection.name + "/" + trackerId, connection.descriptiveName + "/" + trackerId, this); + connection.sensors.put(trackerId, imu); + ReferenceAdjustedTracker adjustedTracker = new ReferenceAdjustedTracker<>(imu); + trackersConsumer.accept(adjustedTracker); + LogManager.log.info("[TrackerServer] Added sensor " + trackerId + " for " + connection.name + ", type " + sensorType); + } + TrackerStatus status = UDPPacket15SensorInfo.getStatus(sensorStatus); + if(status != null) + imu.setStatus(status); + } + + @Override + public void run() { + StringBuilder serialBuffer2 = new StringBuilder(); + try { + socket = new DatagramSocket(port); + + long prevPacketTime = System.currentTimeMillis(); + socket.setSoTimeout(250); + while(true) { + DatagramPacket received = null; + try { + boolean hasActiveTrackers = false; + for(TrackerUDPConnection tracker : connections) { + if(tracker.sensors.size() > 0) { + hasActiveTrackers = true; + break; + } + } + if(!hasActiveTrackers) { + long discoveryPacketTime = System.currentTimeMillis(); + if((discoveryPacketTime - prevPacketTime) >= 2000) { + for(SocketAddress addr : broadcastAddresses) { + bb.limit(bb.capacity()); + bb.rewind(); + parser.write(bb, null, new UDPPacket0Heartbeat()); + socket.send(new DatagramPacket(rcvBuffer, bb.position(), addr)); + } + prevPacketTime = discoveryPacketTime; + } + } + + received = new DatagramPacket(rcvBuffer, rcvBuffer.length); + socket.receive(received); + bb.limit(received.getLength()); + bb.rewind(); + + TrackerUDPConnection connection; + + synchronized(connections) { + connection = connectionsByAddress.get(received.getAddress()); + } + UDPPacket packet = parser.parse(bb, connection); + if(packet != null) { + processPacket(received, packet, connection); + } + } catch(SocketTimeoutException e) { + } catch(Exception e) { + LogManager.log.warning("Error parsing packet " + packetToString(received), e); + } + if(lastKeepup + 500 < System.currentTimeMillis()) { + lastKeepup = System.currentTimeMillis(); + synchronized(connections) { + for(int i = 0; i < connections.size(); ++i) { + TrackerUDPConnection conn = connections.get(i); + bb.limit(bb.capacity()); + bb.rewind(); + parser.write(bb, conn, new UDPPacket1Heartbeat()); + socket.send(new DatagramPacket(rcvBuffer, bb.position(), conn.address)); + if(conn.lastPacket + 1000 < System.currentTimeMillis()) { + Iterator iterator = conn.sensors.values().iterator(); + while(iterator.hasNext()) { + IMUTracker tracker = iterator.next(); + if(tracker.getStatus() == TrackerStatus.OK) + tracker.setStatus(TrackerStatus.DISCONNECTED); + } + } else { + Iterator iterator = conn.sensors.values().iterator(); + while(iterator.hasNext()) { + IMUTracker tracker = iterator.next(); + if(tracker.getStatus() == TrackerStatus.DISCONNECTED) + tracker.setStatus(TrackerStatus.OK); + } + } + if(conn.serialBuffer.length() > 0) { + if(conn.lastSerialUpdate + 500L < System.currentTimeMillis()) { + serialBuffer2.append('[').append(conn.name).append("] ").append(conn.serialBuffer); + System.out.println(serialBuffer2.toString()); + serialBuffer2.setLength(0); + conn.serialBuffer.setLength(0); + } + } + if(conn.lastPingPacketTime + 500 < System.currentTimeMillis()) { + conn.lastPingPacketId = random.nextInt(); + conn.lastPingPacketTime = System.currentTimeMillis(); + bb.limit(bb.capacity()); + bb.rewind(); + bb.putInt(10); + bb.putLong(0); + bb.putInt(conn.lastPingPacketId); + socket.send(new DatagramPacket(rcvBuffer, bb.position(), conn.address)); + } + } + } + } + } + } catch(Exception e) { + e.printStackTrace(); + } finally { + Util.close(socket); + } + } + + protected void processPacket(DatagramPacket received, UDPPacket packet, TrackerUDPConnection connection) throws IOException { + IMUTracker tracker = null; + switch(packet.getPacketId()) { + case UDPProtocolParser.PACKET_HEARTBEAT: + break; + case UDPProtocolParser.PACKET_HANDSHAKE: + setUpNewConnection(received, (UDPPacket3Handshake) packet); + break; + case UDPProtocolParser.PACKET_ROTATION: + case UDPProtocolParser.PACKET_ROTATION_2: + if(connection == null) + break; + UDPPacket1Rotation rotationPacket = (UDPPacket1Rotation) packet; + buf.set(rotationPacket.rotation); + offset.mult(buf, buf); + tracker = connection.sensors.get(rotationPacket.getSensorId()); + if(tracker == null) + break; + tracker.rotQuaternion.set(buf); + tracker.dataTick(); + break; + case UDPProtocolParser.PACKET_ROTATION_DATA: + if(connection == null) + break; + UDPPacket17RotationData rotationData = (UDPPacket17RotationData) packet; + tracker = connection.sensors.get(rotationData.getSensorId()); + if(tracker == null) + break; + buf.set(rotationData.rotation); + offset.mult(buf, buf); + + switch(rotationData.dataType) { + case UDPPacket17RotationData.DATA_TYPE_NORMAL: + tracker.rotQuaternion.set(buf); + tracker.calibrationStatus = rotationData.calibrationInfo; + tracker.dataTick(); + break; + case UDPPacket17RotationData.DATA_TYPE_CORRECTION: + tracker.rotMagQuaternion.set(buf); + tracker.magCalibrationStatus = rotationData.calibrationInfo; + tracker.hasNewCorrectionData = true; + break; + } + break; + case UDPProtocolParser.PACKET_MAGNETOMETER_ACCURACY: + if(connection == null) + break; + UDPPacket18MagnetometerAccuracy magAccuracy = (UDPPacket18MagnetometerAccuracy) packet; + tracker = connection.sensors.get(magAccuracy.getSensorId()); + if(tracker == null) + break; + tracker.magnetometerAccuracy = magAccuracy.accuracyInfo; + break; + case 2: // PACKET_GYRO + case 4: // PACKET_ACCEL + case 5: // PACKET_MAG + case 9: // PACKET_RAW_MAGENTOMETER + break; // None of these packets are used by SlimeVR trackers and are deprecated, use more generic PACKET_ROTATION_DATA + case 8: // PACKET_CONFIG + if(connection == null) + break; + break; + case UDPProtocolParser.PACKET_PING_PONG: // PACKET_PING_PONG: + if(connection == null) + break; + UDPPacket10PingPong ping = (UDPPacket10PingPong) packet; + if(connection.lastPingPacketId == ping.pingId) { + for(int i = 0; i < connection.sensors.size(); ++i) { + tracker = connection.sensors.get(i); + tracker.ping = (int) (System.currentTimeMillis() - connection.lastPingPacketTime) / 2; + tracker.dataTick(); + } + } else { + LogManager.log.debug("[TrackerServer] Wrog ping id " + ping.pingId + " != " + connection.lastPingPacketId); + } + break; + case UDPProtocolParser.PACKET_SERIAL: + if(connection == null) + break; + UDPPacket11Serial serial = (UDPPacket11Serial) packet; + System.out.println("[" + connection.name + "] " + serial.serial); + break; + case UDPProtocolParser.PACKET_BATTERY_LEVEL: + if(connection == null) + break; + UDPPacket12BatteryLevel battery = (UDPPacket12BatteryLevel) packet; + if(connection.sensors.size() > 0) { + Collection trackers = connection.sensors.values(); + Iterator iterator = trackers.iterator(); + while(iterator.hasNext()) { + IMUTracker tr = iterator.next(); + tr.setBatteryVoltage(battery.voltage); + tr.setBatteryLevel(battery.level * 100); + } + } + break; + case UDPProtocolParser.PACKET_TAP: + if(connection == null) + break; + UDPPacket13Tap tap = (UDPPacket13Tap) packet; + tracker = connection.sensors.get(tap.getSensorId()); + if(tracker == null) + break; + LogManager.log.info("[TrackerServer] Tap packet received from " + tracker.getName() + ": " + tap.tap); + break; + case UDPProtocolParser.PACKET_ERROR: + UDPPacket14Error error = (UDPPacket14Error) packet; + LogManager.log.severe("[TrackerServer] Error recieved from " + received.getSocketAddress() + ": " + error.errorNumber); + if(connection == null) + break; + tracker = connection.sensors.get(error.getSensorId()); + if(tracker == null) + break; + tracker.setStatus(TrackerStatus.ERROR); + break; + case UDPProtocolParser.PACKET_SENSOR_INFO: + if(connection == null) + break; + UDPPacket15SensorInfo info = (UDPPacket15SensorInfo) packet; + setUpSensor(connection, info.getSensorId(), info.sensorType, info.sensorStatus); + // Send ack + bb.limit(bb.capacity()); + bb.rewind(); + parser.writeSensorInfoResponse(bb, connection, info); + socket.send(new DatagramPacket(rcvBuffer, bb.position(), connection.address)); + LogManager.log.info("[TrackerServer] Sensor info for " + connection.descriptiveName + "/" + info.getSensorId() + ": " + info.sensorStatus); + break; + case UDPProtocolParser.PACKET_SIGNAL_STRENGTH: + if(connection == null) + break; + UDPPacket19SignalStrength signalStrength = (UDPPacket19SignalStrength) packet; + if(connection.sensors.size() > 0) { + Collection trackers = connection.sensors.values(); + Iterator iterator = trackers.iterator(); + while(iterator.hasNext()) { + IMUTracker tr = iterator.next(); + tr.signalStrength = signalStrength.signalStrength; + } + } + break; + case UDPProtocolParser.PACKET_TEMPERATURE: + if(connection == null) + break; + UDPPacket20Temperature temp = (UDPPacket20Temperature) packet; + tracker = connection.sensors.get(temp.getSensorId()); + if(tracker == null) + break; + tracker.temperature = temp.temperature; + break; + default: + LogManager.log.warning("[TrackerServer] Skipped packet " + packet); + break; + } + } + + private static String packetToString(DatagramPacket packet) { + StringBuilder sb = new StringBuilder(); + sb.append("DatagramPacket{"); + sb.append(packet.getAddress().toString()); + sb.append(packet.getPort()); + sb.append(','); + sb.append(packet.getLength()); + sb.append(','); + sb.append(ArrayUtils.toString(packet.getData())); + sb.append('}'); + return sb.toString(); + } +} diff --git a/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket.java b/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket.java new file mode 100644 index 0000000000..1754c043f5 --- /dev/null +++ b/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket.java @@ -0,0 +1,70 @@ +package dev.slimevr.vr.trackers.udp; + +import java.io.IOException; +import java.nio.ByteBuffer; + +public abstract class UDPPacket { + + public abstract int getPacketId(); + + public abstract void readData(ByteBuffer buf) throws IOException; + + public abstract void writeData(ByteBuffer buf) throws IOException; + + @Override + public String toString() { + StringBuilder sb = new StringBuilder(); + sb.append(getClass().getSimpleName()); + sb.append('{'); + sb.append(getPacketId()); + if(this instanceof SensorSpecificPacket) { + sb.append(",sensor:"); + sb.append(((SensorSpecificPacket) this).getSensorId()); + } + sb.append('}'); + return sb.toString(); + } + + /** + * Naively read null-terminated ASCII string from the byte buffer + * @param buf + * @return + * @throws IOException + */ + public static String readASCIIString(ByteBuffer buf) throws IOException { + StringBuilder sb = new StringBuilder(); + while(true) { + char c = (char) (buf.get() & 0xFF); + if(c == 0) + break; + sb.append(c); + } + return sb.toString(); + } + + public static String readASCIIString(ByteBuffer buf, int length) throws IOException { + StringBuilder sb = new StringBuilder(); + while(length-- > 0) { + char c = (char) (buf.get() & 0xFF); + if(c == 0) + break; + sb.append(c); + } + return sb.toString(); + } + + /** + * Naively write null-terminated ASCII string to byte buffer + * @param str + * @param buf + * @throws IOException + */ + public static void writeASCIIString(String str, ByteBuffer buf) throws IOException { + for(int i = 0; i < str.length(); ++i) { + char c = str.charAt(i); + buf.put((byte) (c & 0xFF)); + } + buf.put((byte) 0); + } + +} diff --git a/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket0Heartbeat.java b/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket0Heartbeat.java new file mode 100644 index 0000000000..5d87a71a35 --- /dev/null +++ b/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket0Heartbeat.java @@ -0,0 +1,25 @@ +package dev.slimevr.vr.trackers.udp; + +import java.io.IOException; +import java.nio.ByteBuffer; + +public class UDPPacket0Heartbeat extends UDPPacket { + + public UDPPacket0Heartbeat() { + } + + @Override + public int getPacketId() { + return 0; + } + + @Override + public void readData(ByteBuffer buf) throws IOException { + // Empty packet + } + + @Override + public void writeData(ByteBuffer buf) throws IOException { + // Empty packet + } +} diff --git a/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket10PingPong.java b/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket10PingPong.java new file mode 100644 index 0000000000..a642959443 --- /dev/null +++ b/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket10PingPong.java @@ -0,0 +1,32 @@ +package dev.slimevr.vr.trackers.udp; + +import java.io.IOException; +import java.nio.ByteBuffer; + +public class UDPPacket10PingPong extends UDPPacket { + + public int pingId; + + public UDPPacket10PingPong() { + } + + public UDPPacket10PingPong(int pingId) { + this.pingId = pingId; + } + + @Override + public int getPacketId() { + return 10; + } + + @Override + public void readData(ByteBuffer buf) throws IOException { + pingId = buf.getInt(); + } + + @Override + public void writeData(ByteBuffer buf) throws IOException { + buf.putInt(pingId); + } + +} diff --git a/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket11Serial.java b/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket11Serial.java new file mode 100644 index 0000000000..735c2ac943 --- /dev/null +++ b/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket11Serial.java @@ -0,0 +1,34 @@ +package dev.slimevr.vr.trackers.udp; + +import java.io.IOException; +import java.nio.ByteBuffer; + +public class UDPPacket11Serial extends UDPPacket { + + public String serial; + + public UDPPacket11Serial() { + } + + @Override + public int getPacketId() { + return 11; + } + + @Override + public void readData(ByteBuffer buf) throws IOException { + int length = buf.getInt(); + StringBuilder sb = new StringBuilder(length); + for(int i = 0; i < length; ++i) { + char ch = (char) buf.get(); + sb.append(ch); + } + serial = sb.toString(); + } + + @Override + public void writeData(ByteBuffer buf) throws IOException { + // Never sent back in current protocol + } + +} diff --git a/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket12BatteryLevel.java b/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket12BatteryLevel.java new file mode 100644 index 0000000000..73bc1584a9 --- /dev/null +++ b/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket12BatteryLevel.java @@ -0,0 +1,30 @@ +package dev.slimevr.vr.trackers.udp; + +import java.io.IOException; +import java.nio.ByteBuffer; + +public class UDPPacket12BatteryLevel extends UDPPacket { + + public float voltage; + public float level; + + public UDPPacket12BatteryLevel() { + } + + @Override + public int getPacketId() { + return 12; + } + + @Override + public void readData(ByteBuffer buf) throws IOException { + voltage = buf.getFloat(); + if(buf.remaining() > 3) + level = buf.getFloat(); + } + + @Override + public void writeData(ByteBuffer buf) throws IOException { + // Never sent back in current protocol + } +} diff --git a/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket13Tap.java b/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket13Tap.java new file mode 100644 index 0000000000..3fe9cf26b2 --- /dev/null +++ b/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket13Tap.java @@ -0,0 +1,36 @@ +package dev.slimevr.vr.trackers.udp; + +import java.io.IOException; +import java.nio.ByteBuffer; + +import dev.slimevr.vr.trackers.SensorTap; + +public class UDPPacket13Tap extends UDPPacket implements SensorSpecificPacket { + + public int sensorId; + public SensorTap tap; + + public UDPPacket13Tap() { + } + + @Override + public int getPacketId() { + return 13; + } + + @Override + public void readData(ByteBuffer buf) throws IOException { + sensorId = buf.get() & 0xFF; + tap = new SensorTap(buf.get() & 0xFF); + } + + @Override + public void writeData(ByteBuffer buf) throws IOException { + // Never sent back in current protocol + } + + @Override + public int getSensorId() { + return sensorId; + } +} diff --git a/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket14Error.java b/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket14Error.java new file mode 100644 index 0000000000..4b4eaa49a2 --- /dev/null +++ b/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket14Error.java @@ -0,0 +1,35 @@ +package dev.slimevr.vr.trackers.udp; + +import java.io.IOException; +import java.nio.ByteBuffer; + +public class UDPPacket14Error extends UDPPacket implements SensorSpecificPacket { + + public int sensorId; + public int errorNumber; + + public UDPPacket14Error() { + } + + @Override + public int getPacketId() { + return 14; + } + + @Override + public void readData(ByteBuffer buf) throws IOException { + sensorId = buf.get() & 0xFF; + errorNumber = buf.get() & 0xFF; + } + + @Override + public void writeData(ByteBuffer buf) throws IOException { + // Never sent back in current protocol + } + + @Override + public int getSensorId() { + return sensorId; + } + +} diff --git a/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket15SensorInfo.java b/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket15SensorInfo.java new file mode 100644 index 0000000000..f8ec824273 --- /dev/null +++ b/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket15SensorInfo.java @@ -0,0 +1,51 @@ +package dev.slimevr.vr.trackers.udp; + +import java.io.IOException; +import java.nio.ByteBuffer; + +import dev.slimevr.vr.trackers.TrackerStatus; + +public class UDPPacket15SensorInfo extends UDPPacket implements SensorSpecificPacket { + + public int sensorId; + public int sensorStatus; + public int sensorType; + + public UDPPacket15SensorInfo() { + } + + @Override + public int getPacketId() { + return 15; + } + + @Override + public void readData(ByteBuffer buf) throws IOException { + sensorId = buf.get() & 0xFF; + sensorStatus = buf.get() & 0xFF; + sensorType = buf.get() & 0xFF; + } + + @Override + public void writeData(ByteBuffer buf) throws IOException { + // Never sent back in current protocol + } + + @Override + public int getSensorId() { + return sensorId; + } + + public static TrackerStatus getStatus(int sensorStatus) { + switch(sensorStatus) { + case 0: + return TrackerStatus.DISCONNECTED; + case 1: + return TrackerStatus.OK; + case 2: + return TrackerStatus.ERROR; + } + return null; + } + +} diff --git a/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket16Rotation2.java b/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket16Rotation2.java new file mode 100644 index 0000000000..e32a759b08 --- /dev/null +++ b/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket16Rotation2.java @@ -0,0 +1,17 @@ +package dev.slimevr.vr.trackers.udp; + +public class UDPPacket16Rotation2 extends UDPPacket1Rotation { + + public UDPPacket16Rotation2() { + } + + @Override + public int getPacketId() { + return 16; + } + + @Override + public int getSensorId() { + return 2; + } +} diff --git a/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket17RotationData.java b/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket17RotationData.java new file mode 100644 index 0000000000..b5896e70e8 --- /dev/null +++ b/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket17RotationData.java @@ -0,0 +1,43 @@ +package dev.slimevr.vr.trackers.udp; + +import java.io.IOException; +import java.nio.ByteBuffer; + +import com.jme3.math.Quaternion; + +public class UDPPacket17RotationData extends UDPPacket implements SensorSpecificPacket { + + public static final int DATA_TYPE_NORMAL = 1; + public static final int DATA_TYPE_CORRECTION = 2; + + public int sensorId; + public int dataType; + public final Quaternion rotation = new Quaternion(); + public int calibrationInfo; + + public UDPPacket17RotationData() { + } + + @Override + public int getPacketId() { + return 17; + } + + @Override + public void readData(ByteBuffer buf) throws IOException { + sensorId = buf.get() & 0xFF; + dataType = buf.get() & 0xFF; + rotation.set(buf.getFloat(), buf.getFloat(), buf.getFloat(), buf.getFloat()); + calibrationInfo = buf.get() & 0xFF; + } + + @Override + public void writeData(ByteBuffer buf) throws IOException { + // Never sent back in current protocol + } + + @Override + public int getSensorId() { + return sensorId; + } +} diff --git a/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket18MagnetometerAccuracy.java b/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket18MagnetometerAccuracy.java new file mode 100644 index 0000000000..b55ae34b7e --- /dev/null +++ b/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket18MagnetometerAccuracy.java @@ -0,0 +1,34 @@ +package dev.slimevr.vr.trackers.udp; + +import java.io.IOException; +import java.nio.ByteBuffer; + +public class UDPPacket18MagnetometerAccuracy extends UDPPacket implements SensorSpecificPacket { + + public int sensorId; + public float accuracyInfo; + + public UDPPacket18MagnetometerAccuracy() { + } + + @Override + public int getPacketId() { + return 18; + } + + @Override + public void readData(ByteBuffer buf) throws IOException { + sensorId = buf.get() & 0xFF; + accuracyInfo = buf.getFloat(); + } + + @Override + public void writeData(ByteBuffer buf) throws IOException { + // Never sent back in current protocol + } + + @Override + public int getSensorId() { + return sensorId; + } +} diff --git a/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket19SignalStrength.java b/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket19SignalStrength.java new file mode 100644 index 0000000000..1fd088bb4a --- /dev/null +++ b/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket19SignalStrength.java @@ -0,0 +1,34 @@ +package dev.slimevr.vr.trackers.udp; + +import java.io.IOException; +import java.nio.ByteBuffer; + +public class UDPPacket19SignalStrength extends UDPPacket implements SensorSpecificPacket { + + public int sensorId; + public int signalStrength; + + public UDPPacket19SignalStrength() { + } + + @Override + public int getPacketId() { + return 19; + } + + @Override + public void readData(ByteBuffer buf) throws IOException { + sensorId = buf.get() & 0xFF; + signalStrength = buf.get(); + } + + @Override + public void writeData(ByteBuffer buf) throws IOException { + // Never sent back in current protocol + } + + @Override + public int getSensorId() { + return sensorId; + } +} diff --git a/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket1Heartbeat.java b/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket1Heartbeat.java new file mode 100644 index 0000000000..594906dcd5 --- /dev/null +++ b/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket1Heartbeat.java @@ -0,0 +1,9 @@ +package dev.slimevr.vr.trackers.udp; + +public class UDPPacket1Heartbeat extends UDPPacket0Heartbeat { + + @Override + public int getPacketId() { + return 1; + } +} diff --git a/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket1Rotation.java b/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket1Rotation.java new file mode 100644 index 0000000000..9aff5a4901 --- /dev/null +++ b/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket1Rotation.java @@ -0,0 +1,34 @@ +package dev.slimevr.vr.trackers.udp; + +import java.io.IOException; +import java.nio.ByteBuffer; + +import com.jme3.math.Quaternion; + +public class UDPPacket1Rotation extends UDPPacket implements SensorSpecificPacket { + + public final Quaternion rotation = new Quaternion(); + + public UDPPacket1Rotation() { + } + + @Override + public int getPacketId() { + return 1; + } + + @Override + public void readData(ByteBuffer buf) throws IOException { + rotation.set(buf.getFloat(), buf.getFloat(), buf.getFloat(), buf.getFloat()); + } + + @Override + public void writeData(ByteBuffer buf) throws IOException { + // Never sent back in current protocol + } + + @Override + public int getSensorId() { + return 0; + } +} diff --git a/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket200ProtocolChange.java b/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket200ProtocolChange.java new file mode 100644 index 0000000000..bc8d147b0f --- /dev/null +++ b/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket200ProtocolChange.java @@ -0,0 +1,31 @@ +package dev.slimevr.vr.trackers.udp; + +import java.io.IOException; +import java.nio.ByteBuffer; + +public class UDPPacket200ProtocolChange extends UDPPacket { + + public int targetProtocol; + public int targetProtocolVersion; + + public UDPPacket200ProtocolChange() { + } + + @Override + public int getPacketId() { + return 200; + } + + @Override + public void readData(ByteBuffer buf) throws IOException { + targetProtocol = buf.get() & 0xFF; + targetProtocolVersion = buf.get() & 0xFF; + } + + @Override + public void writeData(ByteBuffer buf) throws IOException { + buf.put((byte) targetProtocol); + buf.put((byte) targetProtocolVersion); + } + +} diff --git a/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket20Temperature.java b/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket20Temperature.java new file mode 100644 index 0000000000..1440de5e2f --- /dev/null +++ b/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket20Temperature.java @@ -0,0 +1,35 @@ +package dev.slimevr.vr.trackers.udp; + +import java.io.IOException; +import java.nio.ByteBuffer; + +public class UDPPacket20Temperature extends UDPPacket implements SensorSpecificPacket { + + public int sensorId; + public float temperature; + + public UDPPacket20Temperature() { + } + + @Override + public int getPacketId() { + return 20; + } + + @Override + public void readData(ByteBuffer buf) throws IOException { + sensorId = buf.get() & 0xFF; + temperature = buf.getFloat(); + } + + @Override + public void writeData(ByteBuffer buf) throws IOException { + // Never sent back in current protocol + } + + @Override + public int getSensorId() { + return sensorId; + } + +} diff --git a/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket3Handshake.java b/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket3Handshake.java new file mode 100644 index 0000000000..e7b03579bf --- /dev/null +++ b/src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket3Handshake.java @@ -0,0 +1,59 @@ +package dev.slimevr.vr.trackers.udp; + +import java.io.IOException; +import java.nio.ByteBuffer; + +public class UDPPacket3Handshake extends UDPPacket { + + public int boardType; + public int imuType; + public int mcuType; + public int firmwareBuild; + public String firmware; + public String macString; + + public UDPPacket3Handshake() { + } + + @Override + public int getPacketId() { + return 3; + } + + @Override + public void readData(ByteBuffer buf) throws IOException { + if(buf.remaining() > 0) { + byte[] mac = new byte[6]; + if(buf.remaining() > 3) + boardType = buf.getInt(); + if(buf.remaining() > 3) + imuType = buf.getInt(); + if(buf.remaining() > 3) + mcuType = buf.getInt(); // MCU TYPE + if(buf.remaining() > 11) { + buf.getInt(); // IMU info + buf.getInt(); + buf.getInt(); + } + if(buf.remaining() > 3) + firmwareBuild = buf.getInt(); + int length = 0; + if(buf.remaining() > 0) + length = buf.get(); // firmware version length is 1 longer than that because it's nul-terminated + firmware = readASCIIString(buf, length); + if(buf.remaining() >= mac.length) { + buf.get(mac); + macString = String.format("%02X:%02X:%02X:%02X:%02X:%02X", mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]); + if(macString.equals("00:00:00:00:00:00")) + macString = null; + } + } + } + + @Override + public void writeData(ByteBuffer buf) throws IOException { + // Never sent back in current protocol + // Handshake for RAW SlimeVR and legacy owoTrack has different packet id byte order from normal packets + // So it's handled by raw protocol call + } +} diff --git a/src/main/java/dev/slimevr/vr/trackers/udp/UDPProtocolParser.java b/src/main/java/dev/slimevr/vr/trackers/udp/UDPProtocolParser.java new file mode 100644 index 0000000000..bdf17d68dd --- /dev/null +++ b/src/main/java/dev/slimevr/vr/trackers/udp/UDPProtocolParser.java @@ -0,0 +1,120 @@ +package dev.slimevr.vr.trackers.udp; + +import java.io.IOException; +import java.io.UnsupportedEncodingException; +import java.nio.ByteBuffer; + +import io.eiren.util.logging.LogManager; + +public class UDPProtocolParser { + + public static final int PACKET_HEARTBEAT = 0; + public static final int PACKET_ROTATION = 1; // Deprecated + //public static final int PACKET_GYRO = 2; // Deprecated + public static final int PACKET_HANDSHAKE = 3; + //public static final int PACKET_ACCEL = 4; // Not parsed by server + //public static final int PACKET_MAG = 5; // Deprecated + //public static final int PACKET_RAW_CALIBRATION_DATA = 6; // Not parsed by server + //public static final int PACKET_CALIBRATION_FINISHED = 7; // Not parsed by server + //public static final int PACKET_CONFIG = 8; // Not parsed by server + //public static final int PACKET_RAW_MAGNETOMETER = 9 // Deprecated + public static final int PACKET_PING_PONG = 10; + public static final int PACKET_SERIAL = 11; + public static final int PACKET_BATTERY_LEVEL = 12; + public static final int PACKET_TAP = 13; + public static final int PACKET_ERROR = 14; + public static final int PACKET_SENSOR_INFO = 15; + public static final int PACKET_ROTATION_2 = 16; // Deprecated + public static final int PACKET_ROTATION_DATA = 17; + public static final int PACKET_MAGNETOMETER_ACCURACY = 18; + public static final int PACKET_SIGNAL_STRENGTH = 19; + public static final int PACKET_TEMPERATURE = 20; + + public static final int PACKET_PROTOCOL_CHANGE = 200; + + private static final byte[] HANDSHAKE_BUFFER = new byte[64]; + + public UDPProtocolParser() { + } + + public UDPPacket parse(ByteBuffer buf, TrackerUDPConnection connection) throws IOException { + int packetId = buf.getInt(); + long packetNumber = buf.getLong(); + if(connection != null) { + if(!connection.isNextPacket(packetNumber)) { + // Skip packet because it's not next + throw new IOException("Out of order packet received: id " + packetId + ", number " + packetNumber + ", last " + connection.lastPacketNumber + ", from " + connection); + } + connection.lastPacket = System.currentTimeMillis(); + } + UDPPacket newPacket = getNewPacket(packetId); + if(newPacket != null) { + newPacket.readData(buf); + } else { + LogManager.log.debug("[UDPPorotocolParser] Skipped packet id " + packetId + " from " + connection); + } + return newPacket; + } + + public void write(ByteBuffer buf, TrackerUDPConnection connection, UDPPacket packet) throws IOException { + buf.putInt(packet.getPacketId()); + buf.putLong(0); // Packet number is always 0 when sending data to trackers + packet.writeData(buf); + } + + public void writeHandshakeResponse(ByteBuffer buf, TrackerUDPConnection connection) throws IOException { + buf.put(HANDSHAKE_BUFFER); + } + + public void writeSensorInfoResponse(ByteBuffer buf, TrackerUDPConnection connection, UDPPacket15SensorInfo packet) throws IOException { + buf.putInt(packet.getPacketId()); + buf.put((byte) packet.sensorId); + buf.put((byte) packet.sensorStatus); + } + + protected UDPPacket getNewPacket(int packetId) { + switch(packetId) { + case PACKET_HEARTBEAT: + return new UDPPacket0Heartbeat(); + case PACKET_ROTATION: + return new UDPPacket1Rotation(); + case PACKET_HANDSHAKE: + return new UDPPacket3Handshake(); + case PACKET_PING_PONG: + return new UDPPacket10PingPong(); + case PACKET_SERIAL: + return new UDPPacket11Serial(); + case PACKET_BATTERY_LEVEL: + return new UDPPacket12BatteryLevel(); + case PACKET_TAP: + return new UDPPacket13Tap(); + case PACKET_ERROR: + return new UDPPacket14Error(); + case PACKET_SENSOR_INFO: + return new UDPPacket15SensorInfo(); + case PACKET_ROTATION_2: + return new UDPPacket16Rotation2(); + case PACKET_ROTATION_DATA: + return new UDPPacket17RotationData(); + case PACKET_MAGNETOMETER_ACCURACY: + return new UDPPacket18MagnetometerAccuracy(); + case PACKET_SIGNAL_STRENGTH: + return new UDPPacket19SignalStrength(); + case PACKET_TEMPERATURE: + return new UDPPacket20Temperature(); + case PACKET_PROTOCOL_CHANGE: + return new UDPPacket200ProtocolChange(); + } + return null; + } + + static { + try { + HANDSHAKE_BUFFER[0] = 3; + byte[] str = "Hey OVR =D 5".getBytes("ASCII"); + System.arraycopy(str, 0, HANDSHAKE_BUFFER, 1, str.length); + } catch(UnsupportedEncodingException e) { + throw new AssertionError(e); + } + } +}