From 3b0acbe406177e01e613d90caf149a9ce1628a56 Mon Sep 17 00:00:00 2001 From: Eiren Rain Date: Sun, 9 Jan 2022 12:25:57 +0200 Subject: [PATCH 01/13] Create new trackers only when sensor info packet received --- .../dev/slimevr/vr/trackers/IMUTracker.java | 2 - .../vr/trackers/TrackersUDPServer.java | 69 ++++++++++--------- 2 files changed, 35 insertions(+), 36 deletions(-) diff --git a/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java b/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java index ad3c845f12..538c8156bf 100644 --- a/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java +++ b/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java @@ -36,8 +36,6 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery { protected BufferedTimer timer = new BufferedTimer(1f); public int ping = -1; - 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/TrackersUDPServer.java b/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java index 9d4fe0aceb..7a8ccacb59 100644 --- a/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java +++ b/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java @@ -13,6 +13,7 @@ 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; @@ -60,15 +61,15 @@ public TrackersUDPServer(int port, String name, Consumer trackersConsum this.trackersConsumer = trackersConsumer; } - private void setUpNewSensor(DatagramPacket handshakePacket, ByteBuffer data) throws IOException { + private void setUpNewTracker(DatagramPacket handshakePacket, ByteBuffer data) throws IOException { System.out.println("[TrackerServer] Handshake recieved from " + handshakePacket.getAddress() + ":" + handshakePacket.getPort()); InetAddress addr = handshakePacket.getAddress(); - TrackerConnection sensor; + TrackerConnection tracker; synchronized(trackers) { - sensor = trackersMap.get(addr); + tracker = trackersMap.get(addr); } - if(sensor == null) { - boolean isOwo = false; + if(tracker == null) { + tracker = new TrackerConnection(handshakePacket.getSocketAddress()); data.getLong(); // Skip packet number int boardType = -1; int imuType = -1; @@ -109,32 +110,27 @@ private void setUpNewSensor(DatagramPacket handshakePacket, ByteBuffer data) thr } if(firmware.length() == 0) { firmware.append("owoTrack"); - isOwo = true; + tracker.isOwoTrack = true; } - 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()); - sensor.isOwoTrack = isOwo; + tracker.name = macString != null ? "udp://" + macString : "udp:/" + handshakePacket.getAddress().toString(); + tracker.descriptiveName = "udp:/" + handshakePacket.getAddress().toString(); int i = 0; synchronized(trackers) { i = trackers.size(); - trackers.add(sensor); - trackersMap.put(addr, sensor); + trackers.add(tracker); + trackersMap.put(addr, tracker); } - System.out.println("[TrackerServer] Sensor " + i + " added with address " + handshakePacket.getSocketAddress() + ". Board type: " + boardType + ", imu type: " + imuType + ", firmware: " + firmware + " (" + firmwareBuild + "), mac: " + macString + ", name: " + trackerName); + System.out.println("[TrackerServer] Sensor " + i + " added with address " + handshakePacket.getSocketAddress() + ". Board type: " + boardType + ", imu type: " + imuType + ", firmware: " + firmware + " (" + firmwareBuild + "), mac: " + macString + ", name: " + tracker.name); } - sensor.sensors.get(0).setStatus(TrackerStatus.OK); + tracker.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()); + private void setUpSensor(TrackerConnection connection, int trackerId) throws IOException { + System.out.println("[TrackerServer] Setting up auxilary sensor for " + connection.name); 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); + 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); @@ -143,7 +139,6 @@ private void setUpAuxilarySensor(TrackerConnection connection, int trackerId) th imu.setStatus(TrackerStatus.OK); } - @Override public void run() { byte[] rcvBuffer = new byte[512]; @@ -212,7 +207,7 @@ public void run() { case 0: break; case 3: - setUpNewSensor(recieve, bb); + setUpNewTracker(recieve, bb); break; case 1: // PACKET_ROTATION case 16: // PACKET_ROTATION_2 @@ -309,27 +304,30 @@ public void run() { break; if(connection.isOwoTrack) 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); + serialBuffer2.append('[').append(connection.name).append("] ").append(connection.serialBuffer); System.out.println(serialBuffer2.toString()); serialBuffer2.setLength(0); - tracker.serialBuffer.setLength(0); + connection.serialBuffer.setLength(0); } else { - tracker.serialBuffer.append(ch); + connection.serialBuffer.append(ch); } } break; case 12: // PACKET_BATTERY_VOLTAGE if(connection == null) break; - tracker = connection.sensors.get(0); bb.getLong(); - tracker.setBatteryVoltage(bb.getFloat()); + if(connection.sensors.size() > 0) { + Collection trackers = connection.sensors.values(); + Iterator iterator = trackers.iterator(); + while(iterator.hasNext()) + iterator.next().setBatteryVoltage(bb.getFloat()); + } break; case 13: // PACKET_TAP if(connection == null) @@ -345,10 +343,10 @@ public void run() { 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 + case 14: // PACKET_ERROR bb.getLong(); byte reason = bb.get(); - System.out.println("[TrackerServer] Reset recieved from " + recieve.getSocketAddress() + ": " + reason); + System.out.println("[TrackerServer] Error recieved from " + recieve.getSocketAddress() + ": " + reason); if(connection == null) break; sensorId = bb.get() & 0xFF; @@ -363,8 +361,8 @@ public void run() { bb.getLong(); sensorId = bb.get() & 0xFF; int sensorStatus = bb.get() & 0xFF; - if(sensorId > 0 && sensorStatus == 1) { - setUpAuxilarySensor(connection, sensorId); + if(sensorStatus == 1) { + setUpSensor(connection, sensorId); } bb.rewind(); bb.putInt(15); @@ -440,9 +438,12 @@ private class TrackerConnection { public int lastPingPacketId = -1; public long lastPingPacketTime = 0; public boolean isOwoTrack = false; + public String name; + public String descriptiveName; + public StringBuilder serialBuffer = new StringBuilder(); + long lastSerialUpdate = 0; - public TrackerConnection(IMUTracker tracker, SocketAddress address) { - this.sensors.put(0, tracker); + public TrackerConnection(SocketAddress address) { this.address = address; } } From 80de57833455d1e9a99b9c68e8d260fb01f7487a Mon Sep 17 00:00:00 2001 From: Eiren Rain Date: Sun, 9 Jan 2022 12:27:32 +0200 Subject: [PATCH 02/13] Fix missing refactoring changes --- .../dev/slimevr/vr/trackers/TrackersUDPServer.java | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java b/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java index 7a8ccacb59..8919b3ddff 100644 --- a/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java +++ b/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java @@ -400,15 +400,12 @@ public void run() { 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); + 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); - tracker.serialBuffer.setLength(0); + conn.serialBuffer.setLength(0); } } if(conn.lastPingPacketTime + 500 < System.currentTimeMillis()) { From f5bfbb13e22701f5b2bb987656df21ada3db67d7 Mon Sep 17 00:00:00 2001 From: Eiren Rain Date: Mon, 10 Jan 2022 12:50:02 +0200 Subject: [PATCH 03/13] Added contributions notice to the README --- README.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/README.md b/README.md index 47dc623baf..16086a7383 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. From 82fdedfa14b690b30a9321fc8bd6d991c4949767 Mon Sep 17 00:00:00 2001 From: Eiren Rain Date: Mon, 10 Jan 2022 12:54:25 +0200 Subject: [PATCH 04/13] Minor changes --- slime-java-commons | 2 +- .../vr/trackers/TrackersUDPServer.java | 24 +++++++++++++------ 2 files changed, 18 insertions(+), 8 deletions(-) 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/vr/trackers/TrackersUDPServer.java b/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java index 8919b3ddff..a1de034f62 100644 --- a/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java +++ b/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java @@ -126,17 +126,27 @@ private void setUpNewTracker(DatagramPacket handshakePacket, ByteBuffer data) th socket.send(new DatagramPacket(HANDSHAKE_BUFFER, HANDSHAKE_BUFFER.length, handshakePacket.getAddress(), handshakePacket.getPort())); } - private void setUpSensor(TrackerConnection connection, int trackerId) throws IOException { - System.out.println("[TrackerServer] Setting up auxilary sensor for " + connection.name); + private void setUpSensor(TrackerConnection connection, int trackerId, int sensorType, int sensorStatus) throws IOException { + System.out.println("[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); - System.out.println("[TrackerServer] Sensor added with address " + imu.getName()); + System.out.println("[TrackerServer] Added up sensor " + trackerId + " for " + connection.name + ", type " + sensorType); + } + switch(sensorStatus) { + case 0: + imu.setStatus(TrackerStatus.DISCONNECTED); + break; + case 1: + imu.setStatus(TrackerStatus.OK); + break; + case 2: + imu.setStatus(TrackerStatus.ERROR); + break; } - imu.setStatus(TrackerStatus.OK); } @Override @@ -361,9 +371,9 @@ public void run() { bb.getLong(); sensorId = bb.get() & 0xFF; int sensorStatus = bb.get() & 0xFF; - if(sensorStatus == 1) { - setUpSensor(connection, sensorId); - } + int sensorType = bb.get() & 0xFF; + setUpSensor(connection, sensorId, sensorType, sensorStatus); + // Send ack bb.rewind(); bb.putInt(15); bb.put((byte) sensorId); From 012cb518b3607684a7b1e60efa054dc0d652e679 Mon Sep 17 00:00:00 2001 From: Eiren Rain Date: Thu, 20 Jan 2022 18:03:53 +0200 Subject: [PATCH 05/13] Fix merge issues, track packets order, improve logging in UDP server --- .../vr/trackers/TrackersUDPServer.java | 69 +++++++++++-------- 1 file changed, 40 insertions(+), 29 deletions(-) diff --git a/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java b/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java index 23d0003fa0..eb71b8e31f 100644 --- a/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java +++ b/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java @@ -28,6 +28,7 @@ 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. @@ -62,7 +63,7 @@ public TrackersUDPServer(int port, String name, Consumer trackersConsum } private void setUpNewTracker(DatagramPacket handshakePacket, ByteBuffer data) throws IOException { - System.out.println("[TrackerServer] Handshake recieved from " + handshakePacket.getAddress() + ":" + handshakePacket.getPort()); + LogManager.log.info("[TrackerServer] Handshake recieved from " + handshakePacket.getAddress() + ":" + handshakePacket.getPort()); InetAddress addr = handshakePacket.getAddress(); TrackerConnection tracker; synchronized(trackers) { @@ -70,7 +71,6 @@ private void setUpNewTracker(DatagramPacket handshakePacket, ByteBuffer data) th } if(tracker == null) { tracker = new TrackerConnection(handshakePacket.getSocketAddress()); - data.getLong(); // Skip packet number int boardType = -1; int imuType = -1; int firmwareBuild = -1; @@ -122,21 +122,21 @@ private void setUpNewTracker(DatagramPacket handshakePacket, ByteBuffer data) th trackers.add(tracker); trackersMap.put(addr, tracker); } - System.out.println("[TrackerServer] Sensor " + i + " added with address " + handshakePacket.getSocketAddress() + ". Board type: " + boardType + ", imu type: " + imuType + ", firmware: " + firmware + " (" + firmwareBuild + "), mac: " + macString + ", name: " + tracker.name); + LogManager.log.info("[TrackerServer] Tracker " + i + " added with address " + handshakePacket.getSocketAddress() + ". Board type: " + boardType + ", imu type: " + imuType + ", firmware: " + firmware + " (" + firmwareBuild + "), mac: " + macString + ", name: " + tracker.name); + // TODO : Set up new sensor for older protocols } - tracker.sensors.get(0).setStatus(TrackerStatus.OK); socket.send(new DatagramPacket(HANDSHAKE_BUFFER, HANDSHAKE_BUFFER.length, handshakePacket.getAddress(), handshakePacket.getPort())); } private void setUpSensor(TrackerConnection connection, int trackerId, int sensorType, int sensorStatus) throws IOException { - System.out.println("[TrackerServer] Sensor " + trackerId + " for " + connection.name + " status: " + sensorStatus); + 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); - System.out.println("[TrackerServer] Added up sensor " + trackerId + " for " + connection.name + ", type " + sensorType); + LogManager.log.info("[TrackerServer] Added sensor " + trackerId + " for " + connection.name + ", type " + sensorType); } switch(sensorStatus) { case 0: @@ -187,16 +187,16 @@ public void run() { while(true) { try { boolean hasActiveTrackers = false; - for (TrackerConnection tracker: trackers) { - if (tracker.sensors.get(0).getStatus() == TrackerStatus.OK) { + for(TrackerConnection tracker : trackers) { + if(tracker.sensors.size() > 0) { hasActiveTrackers = true; break; } } - if (!hasActiveTrackers) { + if(!hasActiveTrackers) { long discoveryPacketTime = System.currentTimeMillis(); - if ((discoveryPacketTime - prevPacketTime) >= 2000) { - for (SocketAddress addr: addresses) { + if((discoveryPacketTime - prevPacketTime) >= 2000) { + for(SocketAddress addr: addresses) { socket.send(new DatagramPacket(dummyPacket, dummyPacket.length, addr)); } prevPacketTime = discoveryPacketTime; @@ -212,10 +212,20 @@ public void run() { synchronized(trackers) { connection = trackersMap.get(recieve.getAddress()); } - if(connection != null) + int packetId = bb.getInt(); + long packetNumber = bb.getLong(); + + if(connection != null) { + if(packetId != 10) { + if(!connection.isNextPacket(packetNumber)) { + // Skip packet because it's not next + LogManager.log.warning("[TrackerServer] Out of order packet received: id " + packetId + ", number " + packetNumber + ", last " + connection.lastPacketNumber + ", from " + recieve.getSocketAddress()); + continue; + } + } connection.lastPacket = System.currentTimeMillis(); - int packetId; - switch(packetId = bb.getInt()) { + } + switch(packetId) { case 0: break; case 3: @@ -225,7 +235,6 @@ public void run() { 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) { @@ -243,12 +252,10 @@ public void run() { break; if(connection.isOwoTrack) 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); @@ -272,7 +279,6 @@ public void run() { break; if(connection.isOwoTrack) break; - bb.getLong(); sensorId = bb.get() & 0xFF; tracker = connection.sensors.get(sensorId); if(tracker == null) @@ -290,7 +296,6 @@ public void run() { break; if(connection.isOwoTrack) break; - bb.getLong(); MPUTracker.ConfigurationData data = new MPUTracker.ConfigurationData(bb); Consumer dataConsumer = calibrationDataRequests.remove(connection.sensors.get(0)); if(dataConsumer != null) { @@ -316,7 +321,6 @@ public void run() { break; if(connection.isOwoTrack) break; - bb.getLong(); int length = bb.getInt(); for(int i = 0; i < length; ++i) { char ch = (char) bb.get(); @@ -333,7 +337,6 @@ public void run() { case 12: // PACKET_BATTERY_VOLTAGE if(connection == null) break; - bb.getLong(); float voltage = bb.getFloat(); float level = bb.getFloat() * 100; // Use default level if recieved 0 if(connection.sensors.size() > 0) { @@ -342,7 +345,7 @@ public void run() { while(iterator.hasNext()) { IMUTracker tr = iterator.next(); tr.setBatteryVoltage(voltage); - tr.setBatteryLevel(bb.getFloat() * 100); + tr.setBatteryLevel(level); } } break; @@ -351,19 +354,17 @@ public void run() { break; if(connection.isOwoTrack) 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) + ")"); + LogManager.log.info("[TrackerServer] Tap packet received from " + tracker.getName() + "/" + sensorId + ": " + tapObj + " (b" + Integer.toBinaryString(tap) + ")"); break; case 14: // PACKET_ERROR - bb.getLong(); byte reason = bb.get(); - System.out.println("[TrackerServer] Error recieved from " + recieve.getSocketAddress() + ": " + reason); + LogManager.log.severe("[TrackerServer] Error recieved from " + recieve.getSocketAddress() + ": " + reason); if(connection == null) break; sensorId = bb.get() & 0xFF; @@ -375,7 +376,6 @@ public void run() { case 15: // PACKET_SENSOR_INFO if(connection == null) break; - bb.getLong(); sensorId = bb.get() & 0xFF; int sensorStatus = bb.get() & 0xFF; int sensorType = bb.get() & 0xFF; @@ -386,7 +386,7 @@ public void run() { 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); + LogManager.log.info("[TrackerServer] Sensor info for " + connection.sensors.get(0).getName() + "/" + sensorId + ": " + sensorStatus); break; case 19: if(connection == null) @@ -400,7 +400,7 @@ public void run() { tracker.signalStrength = signalStrength; break; default: - System.out.println("[TrackerServer] Unknown data received: " + packetId + " from " + recieve.getSocketAddress()); + LogManager.log.warning("[TrackerServer] Unknown data received: " + packetId + " from " + recieve.getSocketAddress()); break; } } catch(SocketTimeoutException e) { @@ -441,6 +441,7 @@ public void run() { conn.lastPingPacketTime = System.currentTimeMillis(); bb.rewind(); bb.putInt(10); + bb.putLong(0); bb.putInt(conn.lastPingPacketId); socket.send(new DatagramPacket(rcvBuffer, bb.position(), conn.address)); } @@ -467,10 +468,20 @@ private class TrackerConnection { public String descriptiveName; public StringBuilder serialBuffer = new StringBuilder(); long lastSerialUpdate = 0; + long lastPacketNumber = -1; public TrackerConnection(SocketAddress address) { this.address = address; } + + public boolean isNextPacket(long packetId) { + if(packetId == 0) + return true; + if(packetId <= lastPacketNumber) // Skip repeated or out-of-order packets + return false; + lastPacketNumber = packetId; + return true; + } } static { From 25f53232cd2d8c1908abbde82b2754c6521e906c Mon Sep 17 00:00:00 2001 From: Eiren Rain Date: Thu, 20 Jan 2022 20:27:23 +0200 Subject: [PATCH 06/13] Fix packet number reading --- .../dev/slimevr/vr/trackers/TrackersUDPServer.java | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java b/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java index eb71b8e31f..3d9dd58d0c 100644 --- a/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java +++ b/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java @@ -213,15 +213,14 @@ public void run() { connection = trackersMap.get(recieve.getAddress()); } int packetId = bb.getInt(); - long packetNumber = bb.getLong(); + // TODO Ping is not working at all + long packetNumber = packetId != 10 ? bb.getLong() : 0; if(connection != null) { - if(packetId != 10) { - if(!connection.isNextPacket(packetNumber)) { - // Skip packet because it's not next - LogManager.log.warning("[TrackerServer] Out of order packet received: id " + packetId + ", number " + packetNumber + ", last " + connection.lastPacketNumber + ", from " + recieve.getSocketAddress()); - continue; - } + if(!connection.isNextPacket(packetNumber)) { + // Skip packet because it's not next + LogManager.log.warning("[TrackerServer] Out of order packet received: id " + packetId + ", number " + packetNumber + ", last " + connection.lastPacketNumber + ", from " + recieve.getSocketAddress()); + continue; } connection.lastPacket = System.currentTimeMillis(); } From af8ce60dbe0db116c25420c7bf063e83d69d1824 Mon Sep 17 00:00:00 2001 From: Eiren Rain Date: Thu, 20 Jan 2022 20:57:03 +0200 Subject: [PATCH 07/13] Fix signal strength reading --- .../slimevr/vr/trackers/TrackersUDPServer.java | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java b/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java index 3d9dd58d0c..1ac40479fc 100644 --- a/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java +++ b/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java @@ -390,13 +390,16 @@ public void run() { case 19: if(connection == null) break; - bb.getLong(); - sensorId = bb.get() & 0xFF; - tracker = connection.sensors.get(sensorId); - if(tracker == null) - break; + sensorId = bb.get() & 0xFF; // This is sent but ignored int signalStrength = bb.get(); - tracker.signalStrength = signalStrength; + if(connection.sensors.size() > 0) { + Collection trackers = connection.sensors.values(); + Iterator iterator = trackers.iterator(); + while(iterator.hasNext()) { + IMUTracker tr = iterator.next(); + tr.signalStrength = signalStrength; + } + } break; default: LogManager.log.warning("[TrackerServer] Unknown data received: " + packetId + " from " + recieve.getSocketAddress()); From 435f5d1751b96894d6fd395a359c7705f41266ed Mon Sep 17 00:00:00 2001 From: Eiren Rain Date: Tue, 25 Jan 2022 21:46:50 +0200 Subject: [PATCH 08/13] Don't create new trackers if tracker's IP changed while server is running, hand over old trackers to the new connection Implements #70 --- .../vr/trackers/TrackersUDPServer.java | 109 ++++++++++-------- 1 file changed, 60 insertions(+), 49 deletions(-) diff --git a/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java b/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java index 1ac40479fc..5890aa6e9d 100644 --- a/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java +++ b/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java @@ -34,7 +34,7 @@ * Recieves trackers data by UDP using extended owoTrack protocol. */ public class TrackersUDPServer extends Thread { - + /** * Change between IMU axises and OpenGL/SteamVR axises */ @@ -44,12 +44,12 @@ public class TrackersUDPServer extends Thread { 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 List connections = new FastList<>(); + private final Map connectionsByAddress = new HashMap<>(); + private final Map connectionsByMAC = new HashMap<>(); private final Consumer trackersConsumer; private final int port; @@ -62,15 +62,15 @@ public TrackersUDPServer(int port, String name, Consumer trackersConsum this.trackersConsumer = trackersConsumer; } - private void setUpNewTracker(DatagramPacket handshakePacket, ByteBuffer data) throws IOException { + private void setUpNewConnection(DatagramPacket handshakePacket, ByteBuffer data) throws IOException { LogManager.log.info("[TrackerServer] Handshake recieved from " + handshakePacket.getAddress() + ":" + handshakePacket.getPort()); InetAddress addr = handshakePacket.getAddress(); - TrackerConnection tracker; - synchronized(trackers) { - tracker = trackersMap.get(addr); + TrackerConnection connection; + synchronized(connections) { + connection = connectionsByAddress.get(addr); } - if(tracker == null) { - tracker = new TrackerConnection(handshakePacket.getSocketAddress()); + if(connection == null) { + connection = new TrackerConnection(handshakePacket.getSocketAddress(), addr); int boardType = -1; int imuType = -1; int firmwareBuild = -1; @@ -112,20 +112,35 @@ private void setUpNewTracker(DatagramPacket handshakePacket, ByteBuffer data) th // TODO Owo track can report firmware // Will be not important after refactoring, but need not forget firmware.append("owoTrack"); - tracker.isOwoTrack = true; + connection.isOwoTrack = true; } - tracker.name = macString != null ? "udp://" + macString : "udp:/" + handshakePacket.getAddress().toString(); - tracker.descriptiveName = "udp:/" + handshakePacket.getAddress().toString(); + connection.name = macString != null ? "udp://" + macString : "udp:/" + handshakePacket.getAddress().toString(); + connection.descriptiveName = "udp:/" + handshakePacket.getAddress().toString(); int i = 0; - synchronized(trackers) { - i = trackers.size(); - trackers.add(tracker); - trackersMap.put(addr, tracker); + synchronized(connections) { + if(macString != null && connectionsByMAC.containsKey(macString)) { + TrackerConnection previousConnection = connectionsByMAC.get(macString); + i = connections.indexOf(previousConnection); + connectionsByAddress.remove(previousConnection.ipAddress); + 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: " + boardType + ", imu type: " + imuType + ", firmware: " + firmware + " (" + firmwareBuild + "), mac: " + macString + ", name: " + previousConnection.name); + } else { + i = connections.size(); + connections.add(connection); + connectionsByAddress.put(addr, connection); + if(macString != null) { + connectionsByMAC.put(macString, connection); + } + LogManager.log.info("[TrackerServer] Tracker " + i + " added with address " + handshakePacket.getSocketAddress() + ". Board type: " + boardType + ", imu type: " + imuType + ", firmware: " + firmware + " (" + firmwareBuild + "), mac: " + macString + ", name: " + connection.name); + } } - LogManager.log.info("[TrackerServer] Tracker " + i + " added with address " + handshakePacket.getSocketAddress() + ". Board type: " + boardType + ", imu type: " + imuType + ", firmware: " + firmware + " (" + firmwareBuild + "), mac: " + macString + ", name: " + tracker.name); - // TODO : Set up new sensor for older protocols + // TODO : Set up new sensor for older protocols } - socket.send(new DatagramPacket(HANDSHAKE_BUFFER, HANDSHAKE_BUFFER.length, handshakePacket.getAddress(), handshakePacket.getPort())); + socket.send(new DatagramPacket(HANDSHAKE_BUFFER, HANDSHAKE_BUFFER.length, handshakePacket.getAddress(), handshakePacket.getPort())); } private void setUpSensor(TrackerConnection connection, int trackerId, int sensorType, int sensorStatus) throws IOException { @@ -158,36 +173,36 @@ public void run() { 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()) { + while(ifaces.hasMoreElements()) { NetworkInterface iface = ifaces.nextElement(); // Ignore loopback, PPP, virtual and disabled devices - if (iface.isLoopback() || !iface.isUp() || iface.isPointToPoint() || iface.isVirtual()) { + if(iface.isLoopback() || !iface.isUp() || iface.isPointToPoint() || iface.isVirtual()) { continue; } Enumeration iaddrs = iface.getInetAddresses(); - while (iaddrs.hasMoreElements()) { + while(iaddrs.hasMoreElements()) { InetAddress iaddr = iaddrs.nextElement(); // Ignore IPv6 addresses - if (iaddr instanceof Inet6Address) { + 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}; - + byte[] dummyPacket = new byte[]{0x0}; + long prevPacketTime = System.currentTimeMillis(); socket.setSoTimeout(250); while(true) { try { boolean hasActiveTrackers = false; - for(TrackerConnection tracker : trackers) { + for(TrackerConnection tracker : connections) { if(tracker.sensors.size() > 0) { hasActiveTrackers = true; break; @@ -196,26 +211,26 @@ public void run() { if(!hasActiveTrackers) { long discoveryPacketTime = System.currentTimeMillis(); if((discoveryPacketTime - prevPacketTime) >= 2000) { - for(SocketAddress addr: addresses) { + 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()); + synchronized(connections) { + connection = connectionsByAddress.get(recieve.getAddress()); } int packetId = bb.getInt(); // TODO Ping is not working at all long packetNumber = packetId != 10 ? bb.getLong() : 0; - + if(connection != null) { if(!connection.isNextPacket(packetNumber)) { // Skip packet because it's not next @@ -228,7 +243,7 @@ public void run() { case 0: break; case 3: - setUpNewTracker(recieve, bb); + setUpNewConnection(recieve, bb); break; case 1: // PACKET_ROTATION case 16: // PACKET_ROTATION_2 @@ -295,11 +310,6 @@ public void run() { break; if(connection.isOwoTrack) break; - 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) @@ -359,7 +369,7 @@ public void run() { break; int tap = bb.get() & 0xFF; BnoTap tapObj = new BnoTap(tap); - LogManager.log.info("[TrackerServer] Tap packet received from " + tracker.getName() + "/" + sensorId + ": " + tapObj + " (b" + Integer.toBinaryString(tap) + ")"); + LogManager.log.info("[TrackerServer] Tap packet received from " + tracker.getName() + "/" + sensorId + ": " + tapObj + " (b" + Integer.toBinaryString(tap) + ")"); break; case 14: // PACKET_ERROR byte reason = bb.get(); @@ -405,15 +415,14 @@ public void run() { LogManager.log.warning("[TrackerServer] Unknown data received: " + packetId + " from " + recieve.getSocketAddress()); break; } - } catch(SocketTimeoutException e) { - } catch(Exception e) { + } 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); + synchronized(connections) { + for(int i = 0; i < connections.size(); ++i) { + TrackerConnection conn = connections.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(); @@ -462,6 +471,7 @@ private class TrackerConnection { Map sensors = new HashMap<>(); SocketAddress address; + InetAddress ipAddress; public long lastPacket = System.currentTimeMillis(); public int lastPingPacketId = -1; public long lastPingPacketTime = 0; @@ -472,8 +482,9 @@ private class TrackerConnection { long lastSerialUpdate = 0; long lastPacketNumber = -1; - public TrackerConnection(SocketAddress address) { + public TrackerConnection(SocketAddress address, InetAddress ipAddress) { this.address = address; + this.ipAddress = ipAddress; } public boolean isNextPacket(long packetId) { @@ -490,7 +501,7 @@ public boolean isNextPacket(long packetId) { try { HANDSHAKE_BUFFER[0] = 3; byte[] str = "Hey OVR =D 5".getBytes("ASCII"); - System.arraycopy(str, 0, HANDSHAKE_BUFFER, 1, str.length); + System.arraycopy(str, 0, HANDSHAKE_BUFFER, 1, str.length); } catch(UnsupportedEncodingException e) { throw new AssertionError(e); } From 4bddb529d48cb64d16faa17a8f9e4bebb9f342f5 Mon Sep 17 00:00:00 2001 From: Eiren Rain Date: Tue, 25 Jan 2022 22:00:02 +0200 Subject: [PATCH 09/13] Fix ping not working --- .../vr/trackers/TrackersUDPServer.java | 29 ++++--------------- 1 file changed, 5 insertions(+), 24 deletions(-) diff --git a/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java b/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java index 5890aa6e9d..34582b6f52 100644 --- a/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java +++ b/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java @@ -108,12 +108,6 @@ private void setUpNewConnection(DatagramPacket handshakePacket, ByteBuffer data) macString = null; } } - if(firmware.length() == 0) { - // TODO Owo track can report firmware - // Will be not important after refactoring, but need not forget - firmware.append("owoTrack"); - connection.isOwoTrack = true; - } connection.name = macString != null ? "udp://" + macString : "udp:/" + handshakePacket.getAddress().toString(); connection.descriptiveName = "udp:/" + handshakePacket.getAddress().toString(); int i = 0; @@ -122,6 +116,7 @@ private void setUpNewConnection(DatagramPacket handshakePacket, ByteBuffer data) TrackerConnection previousConnection = connectionsByMAC.get(macString); i = connections.indexOf(previousConnection); connectionsByAddress.remove(previousConnection.ipAddress); + previousConnection.lastPacketNumber = 0; previousConnection.ipAddress = addr; previousConnection.address = handshakePacket.getSocketAddress(); previousConnection.name = connection.name; @@ -228,8 +223,7 @@ public void run() { connection = connectionsByAddress.get(recieve.getAddress()); } int packetId = bb.getInt(); - // TODO Ping is not working at all - long packetNumber = packetId != 10 ? bb.getLong() : 0; + long packetNumber = bb.getLong(); if(connection != null) { if(!connection.isNextPacket(packetNumber)) { @@ -264,8 +258,6 @@ public void run() { case 17: // PACKET_ROTATION_DATA if(connection == null) break; - if(connection.isOwoTrack) - break; int sensorId = bb.get() & 0xFF; tracker = connection.sensors.get(sensorId); if(tracker == null) @@ -291,8 +283,6 @@ public void run() { case 18: // PACKET_MAGENTOMETER_ACCURACY if(connection == null) break; - if(connection.isOwoTrack) - break; sensorId = bb.get() & 0xFF; tracker = connection.sensors.get(sensorId); if(tracker == null) @@ -308,14 +298,10 @@ public void run() { case 8: // PACKET_CONFIG if(connection == null) break; - if(connection.isOwoTrack) - break; break; case 10: // PACKET_PING_PONG: if(connection == null) break; - if(connection.isOwoTrack) - break; int pingId = bb.getInt(); if(connection.lastPingPacketId == pingId) { for(int i = 0; i < connection.sensors.size(); ++i) { @@ -323,13 +309,13 @@ public void run() { tracker.ping = (int) (System.currentTimeMillis() - connection.lastPingPacketTime) / 2; tracker.dataTick(); } + } else { + LogManager.log.debug("Wrog ping id " + pingId + " != " + connection.lastPingPacketId); } break; case 11: // PACKET_SERIAL if(connection == null) break; - if(connection.isOwoTrack) - break; int length = bb.getInt(); for(int i = 0; i < length; ++i) { char ch = (char) bb.get(); @@ -361,8 +347,6 @@ public void run() { case 13: // PACKET_TAP if(connection == null) break; - if(connection.isOwoTrack) - break; sensorId = bb.get() & 0xFF; tracker = connection.sensors.get(sensorId); if(tracker == null) @@ -475,7 +459,6 @@ private class TrackerConnection { public long lastPacket = System.currentTimeMillis(); public int lastPingPacketId = -1; public long lastPingPacketTime = 0; - public boolean isOwoTrack = false; public String name; public String descriptiveName; public StringBuilder serialBuffer = new StringBuilder(); @@ -488,9 +471,7 @@ public TrackerConnection(SocketAddress address, InetAddress ipAddress) { } public boolean isNextPacket(long packetId) { - if(packetId == 0) - return true; - if(packetId <= lastPacketNumber) // Skip repeated or out-of-order packets + if(packetId != 0 && packetId <= lastPacketNumber) return false; lastPacketNumber = packetId; return true; From 13b37aa2a9bbb278ff87a862be10614b5d7dda94 Mon Sep 17 00:00:00 2001 From: Eiren Rain Date: Thu, 27 Jan 2022 19:31:13 +0200 Subject: [PATCH 10/13] Improved debug, added checkbox to display sensors debug info --- src/main/java/dev/slimevr/Main.java | 2 +- .../java/dev/slimevr/NetworkProtocol.java | 9 ++ .../java/dev/slimevr/gui/TrackersList.java | 62 ++++++++----- .../java/dev/slimevr/gui/VRServerGUI.java | 11 +++ .../trackers/{BnoTap.java => SensorTap.java} | 4 +- .../vr/trackers/TrackersUDPServer.java | 88 ++++++++++++------- 6 files changed, 117 insertions(+), 59 deletions(-) create mode 100644 src/main/java/dev/slimevr/NetworkProtocol.java rename src/main/java/dev/slimevr/vr/trackers/{BnoTap.java => SensorTap.java} (81%) diff --git a/src/main/java/dev/slimevr/Main.java b/src/main/java/dev/slimevr/Main.java index 9ba409b701..a68c13d5e3 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.2.0"; 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/gui/TrackersList.java b/src/main/java/dev/slimevr/gui/TrackersList.java index 9487ad57a0..264022e2a6 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,6 +149,7 @@ private class TrackerPanel extends EJBagNoStretch { JLabel magAccuracy; JLabel adj; JLabel adjYaw; + JLabel adjGyro; JLabel correction; JLabel signalStrength; JLabel rotQuat; @@ -245,18 +253,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 +277,16 @@ 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)); } - //*/ setBorder(BorderFactory.createLineBorder(new Color(0x663399), 2, false)); TrackersList.this.add(this); @@ -314,21 +321,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 +363,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) 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/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 index 34582b6f52..236ee81d9d 100644 --- a/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java +++ b/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java @@ -22,6 +22,8 @@ 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; @@ -44,6 +46,7 @@ public class TrackersUDPServer extends Thread { 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 static final byte[] dummyPacket = new byte[12]; private final Quaternion buf = new Quaternion(); private final Random random = new Random(); @@ -52,6 +55,7 @@ public class TrackersUDPServer extends Thread { private final Map connectionsByMAC = new HashMap<>(); private final Consumer trackersConsumer; private final int port; + private final ArrayList broadcastAddresses = new ArrayList<>(); protected DatagramSocket socket = null; protected long lastKeepup = System.currentTimeMillis(); @@ -60,6 +64,28 @@ public TrackersUDPServer(int port, String name, Consumer trackersConsum 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, ByteBuffer data) throws IOException { @@ -169,32 +195,10 @@ public void run() { 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) { + DatagramPacket received = null; try { boolean hasActiveTrackers = false; for(TrackerConnection tracker : connections) { @@ -206,21 +210,22 @@ public void run() { if(!hasActiveTrackers) { long discoveryPacketTime = System.currentTimeMillis(); if((discoveryPacketTime - prevPacketTime) >= 2000) { - for(SocketAddress addr : addresses) { + for(SocketAddress addr : broadcastAddresses) { socket.send(new DatagramPacket(dummyPacket, dummyPacket.length, addr)); } prevPacketTime = discoveryPacketTime; } } - DatagramPacket recieve = new DatagramPacket(rcvBuffer, rcvBuffer.length); - socket.receive(recieve); + received = new DatagramPacket(rcvBuffer, rcvBuffer.length); + socket.receive(received); + bb.limit(received.getLength()); bb.rewind(); TrackerConnection connection; IMUTracker tracker = null; synchronized(connections) { - connection = connectionsByAddress.get(recieve.getAddress()); + connection = connectionsByAddress.get(received.getAddress()); } int packetId = bb.getInt(); long packetNumber = bb.getLong(); @@ -228,7 +233,7 @@ public void run() { if(connection != null) { if(!connection.isNextPacket(packetNumber)) { // Skip packet because it's not next - LogManager.log.warning("[TrackerServer] Out of order packet received: id " + packetId + ", number " + packetNumber + ", last " + connection.lastPacketNumber + ", from " + recieve.getSocketAddress()); + LogManager.log.warning("[TrackerServer] Out of order packet received: id " + packetId + ", number " + packetNumber + ", last " + connection.lastPacketNumber + ", from " + received.getSocketAddress()); continue; } connection.lastPacket = System.currentTimeMillis(); @@ -237,7 +242,7 @@ public void run() { case 0: break; case 3: - setUpNewConnection(recieve, bb); + setUpNewConnection(received, bb); break; case 1: // PACKET_ROTATION case 16: // PACKET_ROTATION_2 @@ -352,12 +357,12 @@ public void run() { if(tracker == null) break; int tap = bb.get() & 0xFF; - BnoTap tapObj = new BnoTap(tap); + SensorTap tapObj = new SensorTap(tap); LogManager.log.info("[TrackerServer] Tap packet received from " + tracker.getName() + "/" + sensorId + ": " + tapObj + " (b" + Integer.toBinaryString(tap) + ")"); break; case 14: // PACKET_ERROR byte reason = bb.get(); - LogManager.log.severe("[TrackerServer] Error recieved from " + recieve.getSocketAddress() + ": " + reason); + LogManager.log.severe("[TrackerServer] Error recieved from " + received.getSocketAddress() + ": " + reason); if(connection == null) break; sensorId = bb.get() & 0xFF; @@ -396,11 +401,12 @@ public void run() { } break; default: - LogManager.log.warning("[TrackerServer] Unknown data received: " + packetId + " from " + recieve.getSocketAddress()); + LogManager.log.warning("[TrackerServer] Unknown data received: " + packetId + " from " + received.getSocketAddress() + ": " + packetToString(received)); break; } - } catch(SocketTimeoutException e) {} catch(Exception e) { - e.printStackTrace(); + } catch(SocketTimeoutException e) { + } catch(Exception e) { + LogManager.log.warning("Error parsing packet " + packetToString(received), e); } if(lastKeepup + 500 < System.currentTimeMillis()) { lastKeepup = System.currentTimeMillis(); @@ -435,6 +441,7 @@ public void run() { conn.lastPingPacketId = random.nextInt(); conn.lastPingPacketTime = System.currentTimeMillis(); bb.rewind(); + bb.limit(bb.capacity()); bb.putInt(10); bb.putLong(0); bb.putInt(conn.lastPingPacketId); @@ -451,6 +458,19 @@ public void run() { } } + 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(); + } + private class TrackerConnection { Map sensors = new HashMap<>(); From 55e17e76253ffccec45689b4c669a4f9f4ee0cac Mon Sep 17 00:00:00 2001 From: Eiren Rain Date: Thu, 27 Jan 2022 19:52:30 +0200 Subject: [PATCH 11/13] Minor network fixes --- src/main/java/dev/slimevr/Main.java | 2 +- .../vr/trackers/TrackersUDPServer.java | 36 ++++++++++++------- 2 files changed, 25 insertions(+), 13 deletions(-) diff --git a/src/main/java/dev/slimevr/Main.java b/src/main/java/dev/slimevr/Main.java index a68c13d5e3..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.2.0"; + public static String VERSION = "0.1.4"; public static VRServer vrServer; diff --git a/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java b/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java index 236ee81d9d..c7932c4687 100644 --- a/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java +++ b/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java @@ -28,6 +28,7 @@ import com.jme3.math.Quaternion; import com.jme3.math.Vector3f; +import dev.slimevr.NetworkProtocol; import io.eiren.util.Util; import io.eiren.util.collections.FastList; import io.eiren.util.logging.LogManager; @@ -99,7 +100,6 @@ private void setUpNewConnection(DatagramPacket handshakePacket, ByteBuffer data) connection = new TrackerConnection(handshakePacket.getSocketAddress(), addr); int boardType = -1; int imuType = -1; - int firmwareBuild = -1; StringBuilder firmware = new StringBuilder(); byte[] mac = new byte[6]; String macString = null; @@ -116,7 +116,7 @@ private void setUpNewConnection(DatagramPacket handshakePacket, ByteBuffer data) data.getInt(); } if(data.remaining() > 3) - firmwareBuild = data.getInt(); + connection.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 @@ -127,13 +127,19 @@ private void setUpNewConnection(DatagramPacket handshakePacket, ByteBuffer data) firmware.append(c); length--; } - if(data.remaining() > mac.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; } } + if(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 = macString != null ? "udp://" + macString : "udp:/" + handshakePacket.getAddress().toString(); connection.descriptiveName = "udp:/" + handshakePacket.getAddress().toString(); int i = 0; @@ -148,7 +154,7 @@ private void setUpNewConnection(DatagramPacket handshakePacket, ByteBuffer data) 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: " + boardType + ", imu type: " + imuType + ", firmware: " + firmware + " (" + firmwareBuild + "), mac: " + macString + ", name: " + previousConnection.name); + LogManager.log.info("[TrackerServer] Tracker " + i + " handed over to address " + handshakePacket.getSocketAddress() + ". Board type: " + boardType + ", imu type: " + imuType + ", firmware: " + firmware + " (" + connection.firmwareBuild + "), mac: " + macString + ", name: " + previousConnection.name); } else { i = connections.size(); connections.add(connection); @@ -156,10 +162,14 @@ private void setUpNewConnection(DatagramPacket handshakePacket, ByteBuffer data) if(macString != null) { connectionsByMAC.put(macString, connection); } - LogManager.log.info("[TrackerServer] Tracker " + i + " added with address " + handshakePacket.getSocketAddress() + ". Board type: " + boardType + ", imu type: " + imuType + ", firmware: " + firmware + " (" + firmwareBuild + "), mac: " + macString + ", name: " + connection.name); + LogManager.log.info("[TrackerServer] Tracker " + i + " added with address " + handshakePacket.getSocketAddress() + ". Board type: " + boardType + ", imu type: " + imuType + ", firmware: " + firmware + " (" + connection.firmwareBuild + "), mac: " + macString + ", name: " + connection.name); } } - // TODO : Set up new sensor for older protocols + 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, imuType, 1); + } } socket.send(new DatagramPacket(HANDSHAKE_BUFFER, HANDSHAKE_BUFFER.length, handshakePacket.getAddress(), handshakePacket.getPort())); } @@ -476,14 +486,16 @@ private class TrackerConnection { Map sensors = new HashMap<>(); SocketAddress address; 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(); + long lastPacket = System.currentTimeMillis(); + int lastPingPacketId = -1; + long lastPingPacketTime = 0; + String name; + String descriptiveName; + StringBuilder serialBuffer = new StringBuilder(); long lastSerialUpdate = 0; long lastPacketNumber = -1; + NetworkProtocol protocol = null; + int firmwareBuild = 0; public TrackerConnection(SocketAddress address, InetAddress ipAddress) { this.address = address; From 53ca2cf881edc65f7a1670de0465c9cf14047549 Mon Sep 17 00:00:00 2001 From: Eiren Rain Date: Thu, 27 Jan 2022 21:30:25 +0200 Subject: [PATCH 12/13] Move UDP packets and parsing to own calsses Cleanup UDP networking significantly --- src/main/java/dev/slimevr/VRServer.java | 2 +- .../java/dev/slimevr/gui/TrackersList.java | 11 + .../dev/slimevr/vr/trackers/IMUTracker.java | 2 + .../dev/slimevr/vr/trackers/MPUTracker.java | 2 + .../vr/trackers/TrackersUDPServer.java | 527 ------------------ .../vr/trackers/udp/SensorSpecificPacket.java | 15 + .../vr/trackers/udp/TrackerUDPConnection.java | 43 ++ .../vr/trackers/udp/TrackersUDPServer.java | 435 +++++++++++++++ .../slimevr/vr/trackers/udp/UDPPacket.java | 70 +++ .../vr/trackers/udp/UDPPacket0Heartbeat.java | 25 + .../vr/trackers/udp/UDPPacket10PingPong.java | 32 ++ .../vr/trackers/udp/UDPPacket11Serial.java | 34 ++ .../trackers/udp/UDPPacket12BatteryLevel.java | 30 + .../vr/trackers/udp/UDPPacket13Tap.java | 36 ++ .../vr/trackers/udp/UDPPacket14Error.java | 35 ++ .../trackers/udp/UDPPacket15SensorInfo.java | 51 ++ .../vr/trackers/udp/UDPPacket16Rotation2.java | 17 + .../trackers/udp/UDPPacket17RotationData.java | 43 ++ .../udp/UDPPacket18MagnetometerAccuracy.java | 34 ++ .../udp/UDPPacket19SignalStrength.java | 34 ++ .../vr/trackers/udp/UDPPacket1Rotation.java | 34 ++ .../udp/UDPPacket200ProtocolChange.java | 31 ++ .../trackers/udp/UDPPacket20Temperature.java | 35 ++ .../vr/trackers/udp/UDPPacket3Handshake.java | 59 ++ .../vr/trackers/udp/UDPProtocolParser.java | 120 ++++ 25 files changed, 1229 insertions(+), 528 deletions(-) delete mode 100644 src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java create mode 100644 src/main/java/dev/slimevr/vr/trackers/udp/SensorSpecificPacket.java create mode 100644 src/main/java/dev/slimevr/vr/trackers/udp/TrackerUDPConnection.java create mode 100644 src/main/java/dev/slimevr/vr/trackers/udp/TrackersUDPServer.java create mode 100644 src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket.java create mode 100644 src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket0Heartbeat.java create mode 100644 src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket10PingPong.java create mode 100644 src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket11Serial.java create mode 100644 src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket12BatteryLevel.java create mode 100644 src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket13Tap.java create mode 100644 src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket14Error.java create mode 100644 src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket15SensorInfo.java create mode 100644 src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket16Rotation2.java create mode 100644 src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket17RotationData.java create mode 100644 src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket18MagnetometerAccuracy.java create mode 100644 src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket19SignalStrength.java create mode 100644 src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket1Rotation.java create mode 100644 src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket200ProtocolChange.java create mode 100644 src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket20Temperature.java create mode 100644 src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket3Handshake.java create mode 100644 src/main/java/dev/slimevr/vr/trackers/udp/UDPProtocolParser.java 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 264022e2a6..fe43467a54 100644 --- a/src/main/java/dev/slimevr/gui/TrackersList.java +++ b/src/main/java/dev/slimevr/gui/TrackersList.java @@ -154,6 +154,7 @@ private class TrackerPanel extends EJBagNoStretch { JLabel signalStrength; JLabel rotQuat; JLabel rotAdj; + JLabel temperature; @AWTThread public TrackerPanel(Tracker t) { @@ -286,6 +287,8 @@ public void actionPerformed(ActionEvent e) { 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)); @@ -392,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/vr/trackers/IMUTracker.java b/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java index 81bf5bfd3b..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,6 +37,7 @@ 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 TrackerPosition bodyPosition = null; 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/TrackersUDPServer.java b/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java deleted file mode 100644 index c7932c4687..0000000000 --- a/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java +++ /dev/null @@ -1,527 +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.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 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 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 static final byte[] dummyPacket = new byte[12]; - - 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<>(); - - 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, ByteBuffer data) throws IOException { - LogManager.log.info("[TrackerServer] Handshake recieved from " + handshakePacket.getAddress() + ":" + handshakePacket.getPort()); - InetAddress addr = handshakePacket.getAddress(); - TrackerConnection connection; - synchronized(connections) { - connection = connectionsByAddress.get(addr); - } - if(connection == null) { - connection = new TrackerConnection(handshakePacket.getSocketAddress(), addr); - int boardType = -1; - int imuType = -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) - connection.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; - } - } - if(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 = macString != null ? "udp://" + macString : "udp:/" + handshakePacket.getAddress().toString(); - connection.descriptiveName = "udp:/" + handshakePacket.getAddress().toString(); - int i = 0; - synchronized(connections) { - if(macString != null && connectionsByMAC.containsKey(macString)) { - TrackerConnection previousConnection = connectionsByMAC.get(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: " + boardType + ", imu type: " + imuType + ", firmware: " + firmware + " (" + connection.firmwareBuild + "), mac: " + macString + ", name: " + previousConnection.name); - } else { - i = connections.size(); - connections.add(connection); - connectionsByAddress.put(addr, connection); - if(macString != null) { - connectionsByMAC.put(macString, connection); - } - LogManager.log.info("[TrackerServer] Tracker " + i + " added with address " + handshakePacket.getSocketAddress() + ". Board type: " + boardType + ", imu type: " + imuType + ", firmware: " + firmware + " (" + connection.firmwareBuild + "), mac: " + 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, imuType, 1); - } - } - socket.send(new DatagramPacket(HANDSHAKE_BUFFER, HANDSHAKE_BUFFER.length, handshakePacket.getAddress(), handshakePacket.getPort())); - } - - private void setUpSensor(TrackerConnection 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); - } - switch(sensorStatus) { - case 0: - imu.setStatus(TrackerStatus.DISCONNECTED); - break; - case 1: - imu.setStatus(TrackerStatus.OK); - break; - case 2: - imu.setStatus(TrackerStatus.ERROR); - break; - } - } - - @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); - - long prevPacketTime = System.currentTimeMillis(); - socket.setSoTimeout(250); - while(true) { - DatagramPacket received = null; - try { - boolean hasActiveTrackers = false; - for(TrackerConnection tracker : connections) { - if(tracker.sensors.size() > 0) { - hasActiveTrackers = true; - break; - } - } - if(!hasActiveTrackers) { - long discoveryPacketTime = System.currentTimeMillis(); - if((discoveryPacketTime - prevPacketTime) >= 2000) { - for(SocketAddress addr : broadcastAddresses) { - socket.send(new DatagramPacket(dummyPacket, dummyPacket.length, addr)); - } - prevPacketTime = discoveryPacketTime; - } - } - - received = new DatagramPacket(rcvBuffer, rcvBuffer.length); - socket.receive(received); - bb.limit(received.getLength()); - bb.rewind(); - - TrackerConnection connection; - IMUTracker tracker = null; - synchronized(connections) { - connection = connectionsByAddress.get(received.getAddress()); - } - int packetId = bb.getInt(); - long packetNumber = bb.getLong(); - - if(connection != null) { - if(!connection.isNextPacket(packetNumber)) { - // Skip packet because it's not next - LogManager.log.warning("[TrackerServer] Out of order packet received: id " + packetId + ", number " + packetNumber + ", last " + connection.lastPacketNumber + ", from " + received.getSocketAddress()); - continue; - } - connection.lastPacket = System.currentTimeMillis(); - } - switch(packetId) { - case 0: - break; - case 3: - setUpNewConnection(received, bb); - break; - case 1: // PACKET_ROTATION - case 16: // PACKET_ROTATION_2 - if(connection == null) - break; - 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; - 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; - 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; - break; - case 10: // PACKET_PING_PONG: - if(connection == null) - 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(); - } - } else { - LogManager.log.debug("Wrog ping id " + pingId + " != " + connection.lastPingPacketId); - } - break; - case 11: // PACKET_SERIAL - if(connection == null) - break; - int length = bb.getInt(); - for(int i = 0; i < length; ++i) { - char ch = (char) bb.get(); - if(ch == '\n') { - serialBuffer2.append('[').append(connection.name).append("] ").append(connection.serialBuffer); - System.out.println(serialBuffer2.toString()); - serialBuffer2.setLength(0); - connection.serialBuffer.setLength(0); - } else { - connection.serialBuffer.append(ch); - } - } - break; - case 12: // PACKET_BATTERY_VOLTAGE - if(connection == null) - break; - float voltage = bb.getFloat(); - float level = bb.getFloat() * 100; // Use default level if recieved 0 - if(connection.sensors.size() > 0) { - Collection trackers = connection.sensors.values(); - Iterator iterator = trackers.iterator(); - while(iterator.hasNext()) { - IMUTracker tr = iterator.next(); - tr.setBatteryVoltage(voltage); - tr.setBatteryLevel(level); - } - } - break; - case 13: // PACKET_TAP - if(connection == null) - break; - sensorId = bb.get() & 0xFF; - tracker = connection.sensors.get(sensorId); - if(tracker == null) - break; - int tap = bb.get() & 0xFF; - SensorTap tapObj = new SensorTap(tap); - LogManager.log.info("[TrackerServer] Tap packet received from " + tracker.getName() + "/" + sensorId + ": " + tapObj + " (b" + Integer.toBinaryString(tap) + ")"); - break; - case 14: // PACKET_ERROR - byte reason = bb.get(); - LogManager.log.severe("[TrackerServer] Error recieved from " + received.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; - sensorId = bb.get() & 0xFF; - int sensorStatus = bb.get() & 0xFF; - int sensorType = bb.get() & 0xFF; - setUpSensor(connection, sensorId, sensorType, sensorStatus); - // Send ack - bb.rewind(); - bb.putInt(15); - bb.put((byte) sensorId); - bb.put((byte) sensorStatus); - socket.send(new DatagramPacket(rcvBuffer, bb.position(), connection.address)); - LogManager.log.info("[TrackerServer] Sensor info for " + connection.sensors.get(0).getName() + "/" + sensorId + ": " + sensorStatus); - break; - case 19: - if(connection == null) - break; - sensorId = bb.get() & 0xFF; // This is sent but ignored - int signalStrength = bb.get(); - if(connection.sensors.size() > 0) { - Collection trackers = connection.sensors.values(); - Iterator iterator = trackers.iterator(); - while(iterator.hasNext()) { - IMUTracker tr = iterator.next(); - tr.signalStrength = signalStrength; - } - } - break; - default: - LogManager.log.warning("[TrackerServer] Unknown data received: " + packetId + " from " + received.getSocketAddress() + ": " + packetToString(received)); - break; - } - } 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) { - TrackerConnection conn = connections.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); - } - } - 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.rewind(); - bb.limit(bb.capacity()); - 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); - } - } - - 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(); - } - - private class TrackerConnection { - - Map sensors = new HashMap<>(); - SocketAddress address; - InetAddress ipAddress; - long lastPacket = System.currentTimeMillis(); - int lastPingPacketId = -1; - long lastPingPacketTime = 0; - String name; - String descriptiveName; - StringBuilder serialBuffer = new StringBuilder(); - long lastSerialUpdate = 0; - long lastPacketNumber = -1; - NetworkProtocol protocol = null; - int firmwareBuild = 0; - - public TrackerConnection(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; - } - } - - 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..45dae65104 --- /dev/null +++ b/src/main/java/dev/slimevr/vr/trackers/udp/TrackersUDPServer.java @@ -0,0 +1,435 @@ +package dev.slimevr.vr.trackers.udp; + +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.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 static final byte[] HANDSHAKE_BUFFER = new byte[64]; + private static final byte[] KEEPUP_BUFFER = new byte[64]; + private static final byte[] dummyPacket = new byte[12]; + + 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); + } + } + socket.send(new DatagramPacket(HANDSHAKE_BUFFER, HANDSHAKE_BUFFER.length, handshakePacket.getAddress(), handshakePacket.getPort())); + } + + 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) { + socket.send(new DatagramPacket(dummyPacket, dummyPacket.length, 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); + 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); + } + } + 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.rewind(); + bb.limit(bb.capacity()); + 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.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(); + } + + 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; + } +} 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/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); + } + } +} From 930b5c701a90c0cd6ee02db8a777bfb66a373bf7 Mon Sep 17 00:00:00 2001 From: Eiren Rain Date: Thu, 27 Jan 2022 21:38:49 +0200 Subject: [PATCH 13/13] Cleanup UDP responses --- .../vr/trackers/udp/TrackersUDPServer.java | 34 ++++++++----------- .../vr/trackers/udp/UDPPacket1Heartbeat.java | 9 +++++ 2 files changed, 23 insertions(+), 20 deletions(-) create mode 100644 src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket1Heartbeat.java diff --git a/src/main/java/dev/slimevr/vr/trackers/udp/TrackersUDPServer.java b/src/main/java/dev/slimevr/vr/trackers/udp/TrackersUDPServer.java index 45dae65104..7ce32afc82 100644 --- a/src/main/java/dev/slimevr/vr/trackers/udp/TrackersUDPServer.java +++ b/src/main/java/dev/slimevr/vr/trackers/udp/TrackersUDPServer.java @@ -1,7 +1,6 @@ package dev.slimevr.vr.trackers.udp; import java.io.IOException; -import java.io.UnsupportedEncodingException; import java.net.DatagramPacket; import java.net.DatagramSocket; import java.net.Inet6Address; @@ -47,10 +46,6 @@ public class TrackersUDPServer extends Thread { */ 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[] dummyPacket = new byte[12]; - private final Quaternion buf = new Quaternion(); private final Random random = new Random(); private final List connections = new FastList<>(); @@ -141,7 +136,10 @@ private void setUpNewConnection(DatagramPacket handshakePacket, UDPPacket3Handsh setUpSensor(connection, 0, handshake.imuType, 1); } } - socket.send(new DatagramPacket(HANDSHAKE_BUFFER, HANDSHAKE_BUFFER.length, handshakePacket.getAddress(), handshakePacket.getPort())); + 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 { @@ -181,7 +179,10 @@ public void run() { long discoveryPacketTime = System.currentTimeMillis(); if((discoveryPacketTime - prevPacketTime) >= 2000) { for(SocketAddress addr : broadcastAddresses) { - socket.send(new DatagramPacket(dummyPacket, dummyPacket.length, addr)); + bb.limit(bb.capacity()); + bb.rewind(); + parser.write(bb, null, new UDPPacket0Heartbeat()); + socket.send(new DatagramPacket(rcvBuffer, bb.position(), addr)); } prevPacketTime = discoveryPacketTime; } @@ -210,7 +211,10 @@ public void run() { synchronized(connections) { for(int i = 0; i < connections.size(); ++i) { TrackerUDPConnection conn = connections.get(i); - socket.send(new DatagramPacket(KEEPUP_BUFFER, KEEPUP_BUFFER.length, conn.address)); + 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()) { @@ -237,8 +241,8 @@ public void run() { if(conn.lastPingPacketTime + 500 < System.currentTimeMillis()) { conn.lastPingPacketId = random.nextInt(); conn.lastPingPacketTime = System.currentTimeMillis(); - bb.rewind(); bb.limit(bb.capacity()); + bb.rewind(); bb.putInt(10); bb.putLong(0); bb.putInt(conn.lastPingPacketId); @@ -376,6 +380,7 @@ protected void processPacket(DatagramPacket received, UDPPacket packet, TrackerU 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)); @@ -421,15 +426,4 @@ private static String packetToString(DatagramPacket packet) { sb.append('}'); return sb.toString(); } - - 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; - } } 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; + } +}