From dd8f3d86eedfacb8606df8b3581d5bd8314c2669 Mon Sep 17 00:00:00 2001 From: Eiren Rain Date: Sun, 7 Mar 2021 05:55:57 +0300 Subject: [PATCH] Better hips tracking, added batter level support, fix configuration fetchin Update readme --- README.md | 6 +-- src/main/java/io/eiren/gui/TrackersList.java | 11 +++-- .../vr/processor/HumanSekeletonWithLegs.java | 14 ++---- .../vr/processor/HumanSkeleonWithWaist.java | 15 +++--- .../java/io/eiren/vr/trackers/IMUTracker.java | 34 +++++++++++--- .../eiren/vr/trackers/TrackerWithBattery.java | 8 ++++ .../eiren/vr/trackers/TrackersUDPServer.java | 47 ++++++++++++++++--- 7 files changed, 100 insertions(+), 35 deletions(-) create mode 100644 src/main/java/io/eiren/vr/trackers/TrackerWithBattery.java diff --git a/README.md b/README.md index 485b68e45..eb3b79dee 100644 --- a/README.md +++ b/README.md @@ -4,8 +4,8 @@ Server app for SlimeVR ecosystem Server orchestrates communication between VR driver running in SteamVR and multiple sensors. Sensors implementations: -* [owoTrack Mobile App](https://github.com/abb128/owoTrackVRSyncMobile) - use phone as a tracker -* [SlimeVR Tracker for ESP](https://github.com/Eirenliel/SlimeVR-Tracker-ESP) - use EPS8266 and MPU9250 as tracker (supports multiple trackers) +* [SlimeVR Tracker for ESP](https://github.com/SlimeVR/SlimeVR-Tracker-ESP) - EPS8266 microcontroller and multiple IMUs are supported. Future +* [owoTrack Mobile App](https://github.com/abb128/owoTrackVRSyncMobile) - use phone as a tracker (might not work with most recent version due to changes in protocol) Driver implementation: -* Uses [AptrilTags FBT driver](https://github.com/Eirenliel/Simple-OpenVR-Driver-Tutorial) as a driver \ No newline at end of file +* Uses [AptrilTags FBT driver](https://github.com/SlimeVR/Simple-OpenVR-Driver-Tutorial) as a driver (will be replaced soon) \ No newline at end of file diff --git a/src/main/java/io/eiren/gui/TrackersList.java b/src/main/java/io/eiren/gui/TrackersList.java index 00bd7e46e..3df7b9fbe 100644 --- a/src/main/java/io/eiren/gui/TrackersList.java +++ b/src/main/java/io/eiren/gui/TrackersList.java @@ -25,6 +25,7 @@ import io.eiren.vr.trackers.HMDTracker; import io.eiren.vr.trackers.IMUTracker; import io.eiren.vr.trackers.Tracker; import io.eiren.vr.trackers.TrackerConfig; +import io.eiren.vr.trackers.TrackerWithBattery; import io.eiren.vr.trackers.TrackerWithTPS; public class TrackersList extends EJBag { @@ -63,7 +64,8 @@ public class TrackersList extends EJBag { add(new JLabel("Roll"), c(7, 0, 2)); add(new JLabel("Status"), c(8, 0, 2)); add(new JLabel("TPS"), c(9, 0, 2)); - add(new JLabel("Conf"), c(10, 0, 2)); + add(new JLabel("Bat"), c(10, 0, 2)); + trackers.sort((tr1, tr2) -> getTrackerSort(tr1.t) - getTrackerSort(tr2.t)); @@ -130,7 +132,7 @@ public class TrackersList extends EJBag { JLabel a3; JLabel status; JLabel tps; - JLabel conf; + JLabel bat; @AWTThread public TrackerRow(Tracker t) { @@ -152,7 +154,7 @@ public class TrackersList extends EJBag { } else { add(new JLabel(""), c(9, n, 2, GridBagConstraints.FIRST_LINE_START)); } - add(conf = new JLabel("0"), c(10, n, 2, GridBagConstraints.FIRST_LINE_START)); + add(bat = new JLabel("0"), c(10, n, 2, GridBagConstraints.FIRST_LINE_START)); return this; } @@ -175,7 +177,8 @@ public class TrackersList extends EJBag { if(t instanceof TrackerWithTPS) { tps.setText(StringUtils.prettyNumber(((TrackerWithTPS) t).getTPS(), 1)); } - conf.setText(StringUtils.prettyNumber(t.getConfidenceLevel(), 1)); + if(t instanceof TrackerWithBattery) + bat.setText(StringUtils.prettyNumber(((TrackerWithBattery) t).getBatteryVoltage(), 1)); } public int getSize() { diff --git a/src/main/java/io/eiren/vr/processor/HumanSekeletonWithLegs.java b/src/main/java/io/eiren/vr/processor/HumanSekeletonWithLegs.java index ee92a3133..d26a33047 100644 --- a/src/main/java/io/eiren/vr/processor/HumanSekeletonWithLegs.java +++ b/src/main/java/io/eiren/vr/processor/HumanSekeletonWithLegs.java @@ -142,6 +142,8 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist { rightHipNode.localTransform.setRotation(hipBuf); rightKneeNode.localTransform.setRotation(kneeBuf); rightAnkleNode.localTransform.setRotation(kneeBuf); + + // TODO Calculate waist node as some function between waist and hip rotations } // Knee basically has only 1 DoF (pitch), average yaw between knee and hip @@ -149,17 +151,11 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist { hipBuf.toAngles(hipAngles); kneeBuf.toAngles(kneeAngles); - // TODO - hipConfidense = Math.abs(1 - hipConfidense); - kneeConfidense = Math.abs(1 - kneeConfidense); - float confidenseDiff = hipConfidense < kneeConfidense ? 1 : 0;//FloatMath.equalsToZero(hipConfidense) ? 1.0f : FastMath.clamp(0.5f * (kneeConfidense / hipConfidense), 0f, 1f); - - - hipAngles[1] = kneeAngles[1] = interpolateRadians(0.5f, kneeAngles[1], hipAngles[1]); + hipAngles[1] = kneeAngles[1] = interpolateRadians(kneeLerpFactor, kneeAngles[1], hipAngles[1]); //hipAngles[2] = kneeAngles[2] = interpolateRadians(kneeLerpFactor, kneeAngles[2], hipAngles[2]); - hipBuf.fromAngles(hipAngles[0], hipAngles[1], hipAngles[2]); - kneeBuf.fromAngles(kneeAngles[0], kneeAngles[1], kneeAngles[2]); + hipBuf.fromAngles(hipAngles); + kneeBuf.fromAngles(kneeAngles); } public static float normalizeRad(float angle) { diff --git a/src/main/java/io/eiren/vr/processor/HumanSkeleonWithWaist.java b/src/main/java/io/eiren/vr/processor/HumanSkeleonWithWaist.java index bb5044272..39a2689ca 100644 --- a/src/main/java/io/eiren/vr/processor/HumanSkeleonWithWaist.java +++ b/src/main/java/io/eiren/vr/processor/HumanSkeleonWithWaist.java @@ -17,7 +17,8 @@ public class HumanSkeleonWithWaist extends HumanSkeleton { protected final Map configMap = new HashMap<>(); protected final VRServer server; - + + protected final float[] waistAngles = new float[3]; protected final Quaternion qBuf = new Quaternion(); protected final Vector3f vBuf = new Vector3f(); @@ -43,11 +44,11 @@ public class HumanSkeleonWithWaist extends HumanSkeleton { /** * Distacne from eyes to the base of the neck */ - protected float neckLength = 0.2f; + protected float neckLength = 0.1f; /** * Distance from eyes to ear */ - protected float headShift = 0.05f; + protected float headShift = 0.1f; public HumanSkeleonWithWaist(VRServer server, Tracker waistTracker, List computedTrackers) { this.wasitTracker = waistTracker; @@ -144,10 +145,10 @@ public class HumanSkeleonWithWaist extends HumanSkeleton { // Pelvic bone doesn't tilt when humans tilt, unless they really try. // Can't calculate tilt without additional sensors, so just remove it // completely. - //vBuf.set(0, 0, 1); - //qBuf.multLocal(vBuf); - //vBuf.multLocal(1, 0, 1); // Keep only yaw / Don't normalize, it's done by lookAt() - //qBuf.lookAt(vBuf, Vector3f.UNIT_Y); + qBuf.toAngles(waistAngles); + waistAngles[0] = 0; + waistAngles[2] *= 0.2f; // Keep some roll + qBuf.fromAngles(waistAngles); waistNode.localTransform.setRotation(qBuf); } diff --git a/src/main/java/io/eiren/vr/trackers/IMUTracker.java b/src/main/java/io/eiren/vr/trackers/IMUTracker.java index e438da817..980d200e8 100644 --- a/src/main/java/io/eiren/vr/trackers/IMUTracker.java +++ b/src/main/java/io/eiren/vr/trackers/IMUTracker.java @@ -6,9 +6,10 @@ import java.util.function.Consumer; import com.jme3.math.Quaternion; import com.jme3.math.Vector3f; +import io.eiren.math.FloatMath; import io.eiren.util.BufferedTimer; -public class IMUTracker implements Tracker, CalibratingTracker, TrackerWithTPS { +public class IMUTracker implements Tracker, CalibratingTracker, TrackerWithTPS, TrackerWithBattery { public final Vector3f gyroVector = new Vector3f(); public final Vector3f accelVector = new Vector3f(); @@ -19,9 +20,13 @@ public class IMUTracker implements Tracker, CalibratingTracker, TrackerWithTPS { protected final String name; protected final TrackersUDPServer server; protected float confidence = 0; + protected float batteryVoltage = 0; protected BufferedTimer timer = new BufferedTimer(1f); - public CalibrationData newCalibrationData; + public ConfigurationData newCalibrationData; + + public StringBuilder serialBuffer = new StringBuilder(); + long lastSerialUpdate = 0; public IMUTracker(String name, TrackersUDPServer server) { this.name = name; @@ -96,7 +101,21 @@ public class IMUTracker implements Tracker, CalibratingTracker, TrackerWithTPS { this.confidence = newConf; } - public static class CalibrationData { + @Override + public float getBatteryLevel() { + return FloatMath.mapValue(getBatteryVoltage(), 3.6f, 4.2f, 0f, 1f); + } + + @Override + public float getBatteryVoltage() { + return batteryVoltage; + } + + public void setBatteryVoltage(float voltage) { + this.batteryVoltage = voltage; + } + + public static class ConfigurationData { //acel offsets and correction matrix float[] A_B = new float[3]; @@ -106,8 +125,10 @@ public class IMUTracker implements Tracker, CalibratingTracker, TrackerWithTPS { float[][] M_Ainv = new float[3][3]; //raw offsets, determined for gyro at rest float[] G_off = new float[3]; + int deviceId = -1; + int deviceMode = -1; - public CalibrationData(double[] accelBasis, double[] accelAInv, double[] magBasis, double[] magAInv, double[] gyroOffset) { + public ConfigurationData(double[] accelBasis, double[] accelAInv, double[] magBasis, double[] magAInv, double[] gyroOffset) { A_B[0] = (float) accelBasis[0]; A_B[1] = (float) accelBasis[1]; A_B[2] = (float) accelBasis[2]; @@ -141,8 +162,9 @@ public class IMUTracker implements Tracker, CalibratingTracker, TrackerWithTPS { G_off[2] = (float) gyroOffset[2]; } - public CalibrationData(ByteBuffer buffer) { - buffer.getFloat(); // TODO : WHY??? + public ConfigurationData(ByteBuffer buffer) { + deviceMode = buffer.getInt(); + deviceId = buffer.getInt(); // Data is read in reverse, because it was reversed when sending G_off[2] = buffer.getFloat(); G_off[1] = buffer.getFloat(); diff --git a/src/main/java/io/eiren/vr/trackers/TrackerWithBattery.java b/src/main/java/io/eiren/vr/trackers/TrackerWithBattery.java new file mode 100644 index 000000000..e8f4033dd --- /dev/null +++ b/src/main/java/io/eiren/vr/trackers/TrackerWithBattery.java @@ -0,0 +1,8 @@ +package io.eiren.vr.trackers; + +public interface TrackerWithBattery { + + public float getBatteryLevel(); + + public float getBatteryVoltage(); +} diff --git a/src/main/java/io/eiren/vr/trackers/TrackersUDPServer.java b/src/main/java/io/eiren/vr/trackers/TrackersUDPServer.java index ae828e681..3b792dbc0 100644 --- a/src/main/java/io/eiren/vr/trackers/TrackersUDPServer.java +++ b/src/main/java/io/eiren/vr/trackers/TrackersUDPServer.java @@ -20,7 +20,7 @@ import com.jme3.math.Vector3f; import io.eiren.hardware.magentometer.Magneto; import io.eiren.util.Util; import io.eiren.util.collections.FastList; -import io.eiren.vr.trackers.IMUTracker.CalibrationData; +import io.eiren.vr.trackers.IMUTracker.ConfigurationData; /** * Recieves trackers data by UDP using extended owoTrack protocol. @@ -105,7 +105,7 @@ public class TrackersUDPServer extends Thread { } } - public void uploadNewCalibrationData(Tracker tracker, CalibrationData data) { + public void uploadNewCalibrationData(Tracker tracker, ConfigurationData data) { TrackerConnection connection = null; synchronized(trackers) { for(int i = 0; i < trackers.size(); ++i) { @@ -197,7 +197,7 @@ public class TrackersUDPServer extends Thread { System.out.println(String.format("Mag agv {%8.2f, %8.2f, %8.2f},", (magMaxX + magMinX) / 2, (magMaxY + magMinY) / 2, (magMaxZ + magMinZ) / 2)); - IMUTracker.CalibrationData data = new IMUTracker.CalibrationData(accelBasis, accelAInv, magBasis, magAInv, gyroOffset); + IMUTracker.ConfigurationData data = new IMUTracker.ConfigurationData(accelBasis, accelAInv, magBasis, magAInv, gyroOffset); sensor.tracker.newCalibrationData = data; Consumer consumer = newCalibrationDataRequests.remove(sensor.tracker); @@ -233,6 +233,7 @@ public class TrackersUDPServer extends Thread { 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); socket.setSoTimeout(250); @@ -306,7 +307,7 @@ public class TrackersUDPServer extends Thread { if(sensor == null) break; bb.getLong(); - IMUTracker.CalibrationData data = new IMUTracker.CalibrationData(bb); + IMUTracker.ConfigurationData data = new IMUTracker.ConfigurationData(bb); Consumer dataConsumer = calibrationDataRequests.remove(sensor.tracker); if(dataConsumer != null) { dataConsumer.accept(data.toTextMatrix()); @@ -321,6 +322,31 @@ public class TrackersUDPServer extends Thread { float mz = bb.getFloat(); sensor.tracker.confidence = (float) Math.sqrt(mx * mx + my * my + mz * mz); break; + case 11: // PACKET_SERIAL + if(sensor == null) + break; + IMUTracker tracker = sensor.tracker; + bb.getLong(); + int length = bb.getInt(); + for(int i = 0; i < length; ++i) { + char ch = (char) bb.get(); + if(ch == '\n') { + serialBuffer2.append('[').append(tracker.getName()).append("] ").append(tracker.serialBuffer); + System.out.println(serialBuffer2.toString()); + serialBuffer2.setLength(0); + tracker.serialBuffer.setLength(0); + } else { + tracker.serialBuffer.append(ch); + } + } + break; + case 12: // PACKET_BATTERY_VOLTAGE + if(sensor == null) + break; + tracker = sensor.tracker; + bb.getLong(); + tracker.setBatteryVoltage(bb.getFloat()); + break; default: System.out.println("[TrackerServer] Unknown data received: " + packetId + " from " + recieve.getSocketAddress()); break; @@ -334,11 +360,20 @@ public class TrackersUDPServer extends Thread { synchronized(trackers) { for(int i = 0; i < trackers.size(); ++i) { TrackerConnection conn = trackers.get(i); + IMUTracker tracker = conn.tracker; socket.send(new DatagramPacket(KEEPUP_BUFFER, KEEPUP_BUFFER.length, conn.address)); if(conn.lastPacket + 1000 < System.currentTimeMillis()) - conn.tracker.setStatus(TrackerStatus.DISCONNECTED); + tracker.setStatus(TrackerStatus.DISCONNECTED); else - conn.tracker.setStatus(TrackerStatus.OK); + tracker.setStatus(TrackerStatus.OK); + if(tracker.serialBuffer.length() > 0) { + if(tracker.lastSerialUpdate + 500L < System.currentTimeMillis()) { + serialBuffer2.append('[').append(tracker.getName()).append("] ").append(tracker.serialBuffer); + System.out.println(serialBuffer2.toString()); + serialBuffer2.setLength(0); + tracker.serialBuffer.setLength(0); + } + } } } }