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; + } +}