mirror of
https://github.com/SlimeVR/SlimeVR-Server.git
synced 2026-11-21 09:31:20 -08:00
Fix merge issues, track packets order, improve logging in UDP server
This commit is contained in:
1 parent
2d1ffbc5b0
commit
012cb518b3
1 file changed
+40
-29
@@ -28,6 +28,7 @@ import com.jme3.math.Vector3f;
|
||||
|
||||
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 class TrackersUDPServer extends Thread {
|
||||
}
|
||||
|
||||
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 @@ public class TrackersUDPServer extends Thread {
|
||||
}
|
||||
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 @@ public class TrackersUDPServer extends Thread {
|
||||
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<IMUTracker> 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 class TrackersUDPServer extends Thread {
|
||||
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 class TrackersUDPServer extends Thread {
|
||||
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 class TrackersUDPServer extends Thread {
|
||||
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 class TrackersUDPServer extends Thread {
|
||||
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 class TrackersUDPServer extends Thread {
|
||||
break;
|
||||
if(connection.isOwoTrack)
|
||||
break;
|
||||
bb.getLong();
|
||||
sensorId = bb.get() & 0xFF;
|
||||
tracker = connection.sensors.get(sensorId);
|
||||
if(tracker == null)
|
||||
@@ -290,7 +296,6 @@ public class TrackersUDPServer extends Thread {
|
||||
break;
|
||||
if(connection.isOwoTrack)
|
||||
break;
|
||||
bb.getLong();
|
||||
MPUTracker.ConfigurationData data = new MPUTracker.ConfigurationData(bb);
|
||||
Consumer<String> dataConsumer = calibrationDataRequests.remove(connection.sensors.get(0));
|
||||
if(dataConsumer != null) {
|
||||
@@ -316,7 +321,6 @@ public class TrackersUDPServer extends Thread {
|
||||
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 class TrackersUDPServer extends Thread {
|
||||
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 class TrackersUDPServer extends Thread {
|
||||
while(iterator.hasNext()) {
|
||||
IMUTracker tr = iterator.next();
|
||||
tr.setBatteryVoltage(voltage);
|
||||
tr.setBatteryLevel(bb.getFloat() * 100);
|
||||
tr.setBatteryLevel(level);
|
||||
}
|
||||
}
|
||||
break;
|
||||
@@ -351,19 +354,17 @@ public class TrackersUDPServer extends Thread {
|
||||
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 class TrackersUDPServer extends Thread {
|
||||
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 class TrackersUDPServer extends Thread {
|
||||
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 class TrackersUDPServer extends Thread {
|
||||
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 class TrackersUDPServer extends Thread {
|
||||
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 @@ public class TrackersUDPServer extends Thread {
|
||||
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 {
|
||||
|
||||
Reference in new issue
Block a user