From 572dcdf1bb7f97e2c38ac3299ddb4f06444de0aa Mon Sep 17 00:00:00 2001 From: Yury Date: Sun, 2 Jan 2022 23:25:56 +0300 Subject: [PATCH 1/7] Add RSSI to trackers --- src/main/java/dev/slimevr/gui/TrackersList.java | 14 ++++++++++++++ .../java/dev/slimevr/vr/trackers/IMUTracker.java | 1 + .../dev/slimevr/vr/trackers/TrackersUDPServer.java | 13 +++++++++++++ 3 files changed, 28 insertions(+) diff --git a/src/main/java/dev/slimevr/gui/TrackersList.java b/src/main/java/dev/slimevr/gui/TrackersList.java index 3ceffcdf4..c81c0cb16 100644 --- a/src/main/java/dev/slimevr/gui/TrackersList.java +++ b/src/main/java/dev/slimevr/gui/TrackersList.java @@ -143,6 +143,7 @@ public class TrackersList extends EJBoxNoStretch { JLabel adj; JLabel adjYaw; JLabel correction; + JLabel signalStrength; @AWTThread public TrackerPanel(Tracker t) { @@ -216,6 +217,7 @@ public class TrackersList extends EJBoxNoStretch { add(new JLabel("TPS"), c(3, row, 2, GridBagConstraints.FIRST_LINE_START)); if(realTracker instanceof IMUTracker) { add(new JLabel("Ping"), c(2, row, 2, GridBagConstraints.FIRST_LINE_START)); + add(new JLabel("RSSI"), c(4, row, 2, GridBagConstraints.FIRST_LINE_START)); } row++; if(t.hasRotation()) @@ -224,6 +226,7 @@ public class TrackersList extends EJBoxNoStretch { add(position = new JLabel("0 0 0"), c(1, row, 2, GridBagConstraints.FIRST_LINE_START)); if(realTracker instanceof IMUTracker) { add(ping = new JLabel(""), c(2, row, 2, GridBagConstraints.FIRST_LINE_START)); + add(signalStrength = new JLabel(""), c(4, row, 2, GridBagConstraints.FIRST_LINE_START)); } if(realTracker instanceof TrackerWithTPS) { add(tps = new JLabel("0"), c(3, row, 2, GridBagConstraints.FIRST_LINE_START)); @@ -313,6 +316,17 @@ public class TrackersList extends EJBoxNoStretch { if(realTracker instanceof IMUTracker) { if(ping != null) ping.setText(String.valueOf(((IMUTracker) realTracker).ping)); + if (signalStrength != null) { + int signal = ((IMUTracker) realTracker).signalStrength; + if (signal == -1) { + signalStrength.setText("N/A"); + } else { + // -40 dBm is excellent, -95 dBm is very poor + int percentage = (signal - -95) * (100 - 0) / (-40 - -95) + 0; + percentage = Math.max(Math.min(percentage, 100), 0); + signalStrength.setText(String.valueOf(percentage) + "% " + "(" + String.valueOf(signal) + " dBm" + ")"); + } + } } realTracker.getRotation(q); q.toAngles(angles); diff --git a/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java b/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java index ad3c845f1..772121ab2 100644 --- a/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java +++ b/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java @@ -35,6 +35,7 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery { protected BufferedTimer timer = new BufferedTimer(1f); public int ping = -1; + public int signalStrength = -1; public StringBuilder serialBuffer = new StringBuilder(); long lastSerialUpdate = 0; diff --git a/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java b/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java index 9d4fe0ace..1efdbba19 100644 --- a/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java +++ b/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java @@ -373,6 +373,19 @@ public class TrackersUDPServer extends Thread { socket.send(new DatagramPacket(rcvBuffer, bb.position(), connection.address)); System.out.println("[TrackerServer] Sensor info for " + connection.sensors.get(0).getName() + "/" + sensorId + ": " + sensorStatus); break; + case 19: + if(connection == null) + break; + if(connection.isOwoTrack) + break; + bb.getLong(); + sensorId = bb.get() & 0xFF; + tracker = connection.sensors.get(sensorId); + if(tracker == null) + break; + int signalStrength = bb.get(); + tracker.signalStrength = signalStrength; + break; default: System.out.println("[TrackerServer] Unknown data received: " + packetId + " from " + recieve.getSocketAddress()); break; From 27c153f5d397a8e81ec6db0441687cc2067c0903 Mon Sep 17 00:00:00 2001 From: Yury Date: Wed, 12 Jan 2022 19:30:51 +0300 Subject: [PATCH 2/7] Show battery level reported by tracker --- .../java/dev/slimevr/gui/TrackersList.java | 2 +- .../dev/slimevr/vr/trackers/IMUTracker.java | 8 +++- .../vr/trackers/TrackersUDPServer.java | 39 ++++++++++++------- 3 files changed, 32 insertions(+), 17 deletions(-) diff --git a/src/main/java/dev/slimevr/gui/TrackersList.java b/src/main/java/dev/slimevr/gui/TrackersList.java index c81c0cb16..5763b9446 100644 --- a/src/main/java/dev/slimevr/gui/TrackersList.java +++ b/src/main/java/dev/slimevr/gui/TrackersList.java @@ -300,7 +300,7 @@ public class TrackersList extends EJBoxNoStretch { tps.setText(StringUtils.prettyNumber(((TrackerWithTPS) realTracker).getTPS(), 1)); } if(realTracker instanceof TrackerWithBattery) - bat.setText(StringUtils.prettyNumber(((TrackerWithBattery) realTracker).getBatteryVoltage(), 1)); + bat.setText(String.format("%d%% (%sV)", Math.round(((TrackerWithBattery) realTracker).getBatteryLevel()), StringUtils.prettyNumber(((TrackerWithBattery) realTracker).getBatteryVoltage()), 1)); if(t instanceof ReferenceAdjustedTracker) { ((ReferenceAdjustedTracker) t).attachmentFix.toAngles(angles); if(adj != null) diff --git a/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java b/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java index 772121ab2..6ef1e9355 100644 --- a/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java +++ b/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java @@ -4,7 +4,6 @@ import com.jme3.math.FastMath; 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, TrackerWithTPS, TrackerWithBattery { @@ -27,6 +26,7 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery { protected final TrackersUDPServer server; protected float confidence = 0; protected float batteryVoltage = 0; + protected float batteryLevel = 0; public int calibrationStatus = 0; public int magCalibrationStatus = 0; public float magnetometerAccuracy = 0; @@ -150,7 +150,7 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery { @Override public float getBatteryLevel() { - return FloatMath.mapValue(getBatteryVoltage(), 3.6f, 4.2f, 0f, 1f); + return batteryLevel; } @Override @@ -158,6 +158,10 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery { return batteryVoltage; } + public void setBatteryLevel(float level) { + this.batteryLevel = level; + } + public void setBatteryVoltage(float voltage) { this.batteryVoltage = voltage; } diff --git a/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java b/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java index 1efdbba19..318ed2458 100644 --- a/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java +++ b/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java @@ -33,6 +33,12 @@ import io.eiren.util.collections.FastList; */ public class TrackersUDPServer extends Thread { + enum ProtocolCompat { + OWOTRACK_LEGACY, + OWOTRACK8, + SLIMEVR + } + /** * Change between IMU axises and OpenGL/SteamVR axises */ @@ -68,7 +74,6 @@ public class TrackersUDPServer extends Thread { sensor = trackersMap.get(addr); } if(sensor == null) { - boolean isOwo = false; data.getLong(); // Skip packet number int boardType = -1; int imuType = -1; @@ -107,17 +112,19 @@ public class TrackersUDPServer extends Thread { macString = null; } } + ProtocolCompat protocolCompat = ProtocolCompat.SLIMEVR; if(firmware.length() == 0) { firmware.append("owoTrack"); - isOwo = true; + protocolCompat = ProtocolCompat.OWOTRACK_LEGACY; + } else if (firmware.toString().equals("owoTrack8")) { + protocolCompat = ProtocolCompat.OWOTRACK8; } String trackerName = macString != null ? "udp://" + macString : "udp:/" + handshakePacket.getAddress().toString(); String descriptiveName = "udp:/" + handshakePacket.getAddress().toString(); IMUTracker imu = new IMUTracker(Tracker.getNextLocalTrackerId(), trackerName, descriptiveName, this); ReferenceAdjustedTracker adjustedTracker = new ReferenceAdjustedTracker<>(imu); trackersConsumer.accept(adjustedTracker); - sensor = new TrackerConnection(imu, handshakePacket.getSocketAddress()); - sensor.isOwoTrack = isOwo; + sensor = new TrackerConnection(imu, handshakePacket.getSocketAddress(), protocolCompat); int i = 0; synchronized(trackers) { i = trackers.size(); @@ -234,7 +241,7 @@ public class TrackersUDPServer extends Thread { case 17: // PACKET_ROTATION_DATA if(connection == null) break; - if(connection.isOwoTrack) + if(connection.protocol == ProtocolCompat.OWOTRACK_LEGACY) break; bb.getLong(); int sensorId = bb.get() & 0xFF; @@ -263,7 +270,7 @@ public class TrackersUDPServer extends Thread { case 18: // PACKET_MAGENTOMETER_ACCURACY if(connection == null) break; - if(connection.isOwoTrack) + if(connection.protocol == ProtocolCompat.OWOTRACK_LEGACY) break; bb.getLong(); sensorId = bb.get() & 0xFF; @@ -281,7 +288,7 @@ public class TrackersUDPServer extends Thread { case 8: // PACKET_CONFIG if(connection == null) break; - if(connection.isOwoTrack) + if(connection.protocol == ProtocolCompat.OWOTRACK_LEGACY) break; bb.getLong(); MPUTracker.ConfigurationData data = new MPUTracker.ConfigurationData(bb); @@ -293,7 +300,7 @@ public class TrackersUDPServer extends Thread { case 10: // PACKET_PING_PONG: if(connection == null) break; - if(connection.isOwoTrack) + if(connection.protocol == ProtocolCompat.OWOTRACK_LEGACY) break; int pingId = bb.getInt(); if(connection.lastPingPacketId == pingId) { @@ -307,7 +314,7 @@ public class TrackersUDPServer extends Thread { case 11: // PACKET_SERIAL if(connection == null) break; - if(connection.isOwoTrack) + if(connection.protocol == ProtocolCompat.OWOTRACK_LEGACY) break; tracker = connection.sensors.get(0); bb.getLong(); @@ -329,12 +336,17 @@ public class TrackersUDPServer extends Thread { break; tracker = connection.sensors.get(0); bb.getLong(); + if(connection.protocol == ProtocolCompat.OWOTRACK_LEGACY || connection.protocol == ProtocolCompat.OWOTRACK8) { + tracker.setBatteryLevel(bb.getFloat()); + break; + } tracker.setBatteryVoltage(bb.getFloat()); + tracker.setBatteryLevel(bb.getFloat() * 100); break; case 13: // PACKET_TAP if(connection == null) break; - if(connection.isOwoTrack) + if(connection.protocol == ProtocolCompat.OWOTRACK_LEGACY) break; bb.getLong(); sensorId = bb.get() & 0xFF; @@ -376,8 +388,6 @@ public class TrackersUDPServer extends Thread { case 19: if(connection == null) break; - if(connection.isOwoTrack) - break; bb.getLong(); sensorId = bb.get() & 0xFF; tracker = connection.sensors.get(sensorId); @@ -452,11 +462,12 @@ public class TrackersUDPServer extends Thread { public long lastPacket = System.currentTimeMillis(); public int lastPingPacketId = -1; public long lastPingPacketTime = 0; - public boolean isOwoTrack = false; + public ProtocolCompat protocol; - public TrackerConnection(IMUTracker tracker, SocketAddress address) { + public TrackerConnection(IMUTracker tracker, SocketAddress address, ProtocolCompat protocol) { this.sensors.put(0, tracker); this.address = address; + this.protocol = protocol; } } From a8f13bb570643e3024d1fb64434eb0f350f4dbd2 Mon Sep 17 00:00:00 2001 From: Yury Date: Thu, 13 Jan 2022 00:56:08 +0300 Subject: [PATCH 3/7] Fix log in firewall_uninstall.bat --- resources/firewall_uninstall.bat | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/resources/firewall_uninstall.bat b/resources/firewall_uninstall.bat index 115eb1d6a..678450e86 100644 --- a/resources/firewall_uninstall.bat +++ b/resources/firewall_uninstall.bat @@ -1,5 +1,5 @@ @echo off -echo Installing firewall rules... +echo Uninstalling firewall rules... rem Discovery defauly port netsh advfirewall firewall delete rule name="SlimeVR UDP 35903 incoming" From 3b354f103a38c83c597b7521fc48568703e5a436 Mon Sep 17 00:00:00 2001 From: Louka Date: Sat, 15 Jan 2022 18:00:50 -0500 Subject: [PATCH 4/7] Update SkeletonConfigValue.java --- .../vr/processor/skeleton/SkeletonConfigValue.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfigValue.java b/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfigValue.java index 120fbf7e3..95fa5bf80 100644 --- a/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfigValue.java +++ b/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfigValue.java @@ -7,13 +7,13 @@ public enum SkeletonConfigValue { HEAD("Head", "headShift", "Head shift", 0.1f, new SkeletonNodeOffset[]{SkeletonNodeOffset.HEAD}), NECK("Neck", "neckLength", "Neck length", 0.1f, new SkeletonNodeOffset[]{SkeletonNodeOffset.NECK}), - TORSO("Torso", "torsoLength", "Torso length", 0.64f, new SkeletonNodeOffset[]{SkeletonNodeOffset.WAIST}), - CHEST("Chest", "chestDistance", "Chest distance", 0.32f, new SkeletonNodeOffset[]{SkeletonNodeOffset.CHEST, SkeletonNodeOffset.WAIST}), + TORSO("Torso", "torsoLength", "Torso length", 0.6f, new SkeletonNodeOffset[]{SkeletonNodeOffset.WAIST}), + CHEST("Chest", "chestDistance", "Chest distance", 0.3f, new SkeletonNodeOffset[]{SkeletonNodeOffset.CHEST, SkeletonNodeOffset.WAIST}), WAIST("Waist", "waistDistance", "Waist distance", 0.05f, new SkeletonNodeOffset[]{SkeletonNodeOffset.WAIST, SkeletonNodeOffset.HIP}), HIP_OFFSET("Hip offset", "hipOffset", "Hip offset", 0.0f, new SkeletonNodeOffset[]{SkeletonNodeOffset.HIP_TRACKER}), - HIPS_WIDTH("Hips width", "hipsWidth", "Hips width", 0.3f, new SkeletonNodeOffset[]{SkeletonNodeOffset.LEFT_HIP, SkeletonNodeOffset.RIGHT_HIP}), - LEGS_LENGTH("Legs length", "legsLength", "Legs length", 0.86f, new SkeletonNodeOffset[]{SkeletonNodeOffset.KNEE}), - KNEE_HEIGHT("Knee height", "kneeHeight", "Knee height", 0.43f, new SkeletonNodeOffset[]{SkeletonNodeOffset.KNEE, SkeletonNodeOffset.ANKLE}), + HIPS_WIDTH("Hips width", "hipsWidth", "Hips width", 0.28f, new SkeletonNodeOffset[]{SkeletonNodeOffset.LEFT_HIP, SkeletonNodeOffset.RIGHT_HIP}), + LEGS_LENGTH("Legs length", "legsLength", "Legs length", 0.88f, new SkeletonNodeOffset[]{SkeletonNodeOffset.KNEE}), + KNEE_HEIGHT("Knee height", "kneeHeight", "Knee height", 0.44f, new SkeletonNodeOffset[]{SkeletonNodeOffset.KNEE, SkeletonNodeOffset.ANKLE}), FOOT_LENGTH("Foot length", "footLength", "Foot length", 0.05f, new SkeletonNodeOffset[]{SkeletonNodeOffset.FOOT}), FOOT_OFFSET("Foot offset", "footOffset", "Foot offset", 0.0f, new SkeletonNodeOffset[]{SkeletonNodeOffset.ANKLE}), SKELETON_OFFSET("Skeleton offset", "skeletonOffset", "Skeleton offset", 0.0f, new SkeletonNodeOffset[]{SkeletonNodeOffset.CHEST_TRACKER, SkeletonNodeOffset.HIP_TRACKER, SkeletonNodeOffset.KNEE_TRACKER, SkeletonNodeOffset.FOOT_TRACKER}), From a2fc809d71d168f80724c5573ce0fcb0b2147d0f Mon Sep 17 00:00:00 2001 From: Yury Date: Thu, 20 Jan 2022 00:24:47 +0300 Subject: [PATCH 5/7] Fix decimal places for battery voltage --- src/main/java/dev/slimevr/gui/TrackersList.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/dev/slimevr/gui/TrackersList.java b/src/main/java/dev/slimevr/gui/TrackersList.java index 5763b9446..010affbd1 100644 --- a/src/main/java/dev/slimevr/gui/TrackersList.java +++ b/src/main/java/dev/slimevr/gui/TrackersList.java @@ -300,7 +300,7 @@ public class TrackersList extends EJBoxNoStretch { tps.setText(StringUtils.prettyNumber(((TrackerWithTPS) realTracker).getTPS(), 1)); } if(realTracker instanceof TrackerWithBattery) - bat.setText(String.format("%d%% (%sV)", Math.round(((TrackerWithBattery) realTracker).getBatteryLevel()), StringUtils.prettyNumber(((TrackerWithBattery) realTracker).getBatteryVoltage()), 1)); + 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) From 5d49bbfb297891696945eb8ca1ae63ed40930b4f Mon Sep 17 00:00:00 2001 From: Butterscotch! Date: Wed, 19 Jan 2022 19:20:47 -0500 Subject: [PATCH 6/7] Fix AutoBone overwriting configs & improve code documentation --- .../java/dev/slimevr/autobone/AutoBone.java | 41 +++++++++++-------- 1 file changed, 24 insertions(+), 17 deletions(-) diff --git a/src/main/java/dev/slimevr/autobone/AutoBone.java b/src/main/java/dev/slimevr/autobone/AutoBone.java index d10809eb9..18a43aeb2 100644 --- a/src/main/java/dev/slimevr/autobone/AutoBone.java +++ b/src/main/java/dev/slimevr/autobone/AutoBone.java @@ -15,14 +15,13 @@ import dev.slimevr.poserecorder.PoseFrameTracker; import dev.slimevr.poserecorder.PoseFrames; import dev.slimevr.poserecorder.TrackerFrame; import dev.slimevr.poserecorder.TrackerFrameData; +import dev.slimevr.vr.processor.HumanPoseProcessor; import dev.slimevr.vr.processor.skeleton.HumanSkeleton; -import dev.slimevr.vr.processor.skeleton.SimpleSkeleton; import dev.slimevr.vr.processor.skeleton.SkeletonConfig; import dev.slimevr.vr.processor.skeleton.SkeletonConfigValue; import dev.slimevr.vr.trackers.TrackerPosition; import dev.slimevr.vr.trackers.TrackerRole; import dev.slimevr.vr.trackers.TrackerUtils; -import io.eiren.util.ann.ThreadSafe; import io.eiren.util.logging.LogManager; import io.eiren.util.collections.FastList; @@ -77,8 +76,6 @@ public class AutoBone { protected final VRServer server; - protected SimpleSkeleton skeleton = null; - // This is filled by reloadConfigValues() public final EnumMap configs = new EnumMap(SkeletonConfigValue.class); public final EnumMap staticConfigs = new EnumMap(SkeletonConfigValue.class); @@ -90,10 +87,7 @@ public class AutoBone { public AutoBone(VRServer server) { this.server = server; - reloadConfigValues(); - - server.addSkeletonUpdatedCallback(this::skeletonUpdated); } public void reloadConfigValues() { @@ -139,17 +133,18 @@ public class AutoBone { staticConfigs.put(SkeletonConfigValue.SKELETON_OFFSET, 0f); } - @ThreadSafe - public void skeletonUpdated(HumanSkeleton newSkeleton) { - if(newSkeleton instanceof SimpleSkeleton) { - skeleton = (SimpleSkeleton) newSkeleton; - applyConfigToSkeleton(newSkeleton); - LogManager.log.info("[AutoBone] Received updated skeleton"); - } + /** + * A simple utility method to get the {@link HumanSkeleton} from the {@link VRServer} + * @return The {@link HumanSkeleton} associated with the {@link VRServer}, or null if there is none available + * @see {@link VRServer}, {@link HumanSkeleton} + */ + private HumanSkeleton getSkeleton() { + HumanPoseProcessor humanPoseProcessor = server != null ? server.humanPoseProcessor : null; + return humanPoseProcessor != null ? humanPoseProcessor.getSkeleton() : null; } - + public void applyConfig() { - if(!applyConfigToSkeleton(skeleton)) { + if(!applyConfigToSkeleton(getSkeleton())) { // Unable to apply to skeleton, save directly saveConfigs(); } @@ -287,10 +282,14 @@ public class AutoBone { // If target height isn't specified, auto-detect if(targetHeight < 0f) { + // Get the current skeleton from the server + HumanSkeleton skeleton = getSkeleton(); if(skeleton != null) { + // If there is a skeleton available, calculate the target height from its configs targetHeight = sumSelectConfigs(heightConfigs, skeleton.getSkeletonConfig()); LogManager.log.warning("[AutoBone] Target height loaded from skeleton (Make sure you reset before running!): " + targetHeight); } else { + // Otherwise if there is no skeleton available, attempt to get the max HMD height from the recording float hmdHeight = getMaxHmdHeight(frames); if(hmdHeight <= 0.50f) { LogManager.log.warning("[AutoBone] Max headset height detected (Value seems too low, did you not stand up straight while measuring?): " + hmdHeight); @@ -303,12 +302,14 @@ public class AutoBone { } } + // Epoch loop, each epoch is one full iteration over the full dataset for(int epoch = calcInitError ? -1 : 0; epoch < numEpochs; epoch++) { float sumError = 0f; int errorCount = 0; float adjustRate = epoch >= 0 ? (initialAdjustRate / FastMath.pow(adjustRateDecay, epoch)) : 0f; + // Iterate over the frames using a cursor and an offset for comparing frames a certain number of frames apart for(int cursorOffset = minDataDistance; cursorOffset <= maxDataDistance && cursorOffset < frameCount; cursorOffset++) { for(int frameCursor = 0; frameCursor < frameCount - cursorOffset; frameCursor += cursorIncrement) { int frameCursor2 = frameCursor + cursorOffset; @@ -559,42 +560,48 @@ public class AutoBone { float sumWeight = 0f; if(slideErrorFactor > 0f) { + // This is the main error function, this calculates the distance between the foot positions on both frames totalError += getSlideErrorDeriv(skeleton1, skeleton2) * distScale * slideErrorFactor; sumWeight += slideErrorFactor; } if(offsetSlideErrorFactor > 0f) { + // This error function compares the distance between the feet on each frame and returns the offset between them totalError += getOffsetSlideErrorDeriv(skeleton1, skeleton2) * distScale * offsetSlideErrorFactor; sumWeight += offsetSlideErrorFactor; } if(offsetErrorFactor > 0f) { + // This error function compares the height of each foot in each frame totalError += getOffsetErrorDeriv(skeleton1, skeleton2) * distScale * offsetErrorFactor; sumWeight += offsetErrorFactor; } if(proportionErrorFactor > 0f) { + // This error function compares the current values to general expected proportions to keep measurements in line // Either skeleton will work fine, skeleton1 is used as a default totalError += getProportionErrorDeriv(skeleton1.skeletonConfig) * proportionErrorFactor; sumWeight += proportionErrorFactor; } if(heightErrorFactor > 0f) { + // This error function compares the height change to the actual measured height of the headset totalError += FastMath.abs(heightChange) * heightErrorFactor; sumWeight += heightErrorFactor; } if(positionErrorFactor > 0f) { + // This error function compares the position of an assigned tracker with the position on the skeleton totalError += (getPositionErrorDeriv(frames, cursor1, skeleton1) + getPositionErrorDeriv(frames, cursor2, skeleton2) / 2f) * distScale * positionErrorFactor; sumWeight += positionErrorFactor; } if(positionOffsetErrorFactor > 0f) { + // This error function compares the offset of the position of an assigned tracker with the position on the skeleton totalError += getPositionOffsetErrorDeriv(frames, cursor1, cursor2, skeleton1, skeleton2) * distScale * positionOffsetErrorFactor; sumWeight += positionOffsetErrorFactor; } - // Minimize sliding, minimize foot height offset, minimize change in total height return sumWeight > 0f ? totalError / sumWeight : 0f; } From c88a6802a9e3e6c39d8548121e4433a01a221c49 Mon Sep 17 00:00:00 2001 From: Eiren Rain Date: Thu, 20 Jan 2022 17:50:08 +0200 Subject: [PATCH 7/7] Minor debug stuff --- .../java/dev/slimevr/gui/TrackersList.java | 35 ++++++++++++++++--- .../dev/slimevr/vr/trackers/IMUTracker.java | 6 ++-- 2 files changed, 33 insertions(+), 8 deletions(-) diff --git a/src/main/java/dev/slimevr/gui/TrackersList.java b/src/main/java/dev/slimevr/gui/TrackersList.java index 010affbd1..9487ad57a 100644 --- a/src/main/java/dev/slimevr/gui/TrackersList.java +++ b/src/main/java/dev/slimevr/gui/TrackersList.java @@ -144,6 +144,8 @@ public class TrackersList extends EJBoxNoStretch { JLabel adjYaw; JLabel correction; JLabel signalStrength; + JLabel rotQuat; + JLabel rotAdj; @AWTThread public TrackerPanel(Tracker t) { @@ -217,7 +219,7 @@ public class TrackersList extends EJBoxNoStretch { add(new JLabel("TPS"), c(3, row, 2, GridBagConstraints.FIRST_LINE_START)); if(realTracker instanceof IMUTracker) { add(new JLabel("Ping"), c(2, row, 2, GridBagConstraints.FIRST_LINE_START)); - add(new JLabel("RSSI"), c(4, row, 2, GridBagConstraints.FIRST_LINE_START)); + add(new JLabel("Signal"), c(4, row, 2, GridBagConstraints.FIRST_LINE_START)); } row++; if(t.hasRotation()) @@ -243,11 +245,19 @@ public class TrackersList extends EJBoxNoStretch { 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) { + 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) { 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(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)); add(calibration = new JLabel("0"), c(1, row, 2, GridBagConstraints.FIRST_LINE_START)); @@ -256,6 +266,8 @@ public class TrackersList extends EJBoxNoStretch { row++; add(new JLabel("Correction:"), c(0, row, 2, GridBagConstraints.FIRST_LINE_START)); add(correction = new JLabel("0 0 0"), s(c(1, row, 2, GridBagConstraints.FIRST_LINE_START), 3, 1)); + add(new JLabel("RotAdj:"), c(2, row, 2, GridBagConstraints.FIRST_LINE_START)); + add(rotAdj = new JLabel("0"), c(3, row, 2, GridBagConstraints.FIRST_LINE_START)); row++; } //*/ @@ -334,21 +346,34 @@ public class TrackersList extends EJBoxNoStretch { + " " + StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 0) + " " + StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0)); if(realTracker instanceof IMUTracker) { - ((IMUTracker) realTracker).rotMagQuaternion.toAngles(angles); + IMUTracker imu = (IMUTracker) realTracker; + imu.rotMagQuaternion.toAngles(angles); if(rawMag != null) 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(((IMUTracker) realTracker).calibrationStatus + " / " + ((IMUTracker) realTracker).magCalibrationStatus); + calibration.setText(imu.calibrationStatus + " / " + imu.magCalibrationStatus); if(magAccuracy != null) - magAccuracy.setText(StringUtils.prettyNumber(((IMUTracker) realTracker).magnetometerAccuracy * FastMath.RAD_TO_DEG, 1) + "°"); - ((IMUTracker) realTracker).getCorrection(q); + magAccuracy.setText(StringUtils.prettyNumber(imu.magnetometerAccuracy * FastMath.RAD_TO_DEG, 1) + "°"); + imu.getCorrection(q); q.toAngles(angles); if(correction != null) 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) + + " " + StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 0) + + " " + StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0)); + } + if(rotAdj != null) { + imu.rotAdjust.toAngles(angles); + rotAdj.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)); + } } } } diff --git a/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java b/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java index 6ef1e9355..e917186c9 100644 --- a/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java +++ b/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java @@ -10,12 +10,12 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery { public static final float MAX_MAG_CORRECTION_ACCURACY = 5 * FastMath.RAD_TO_DEG; - public final Vector3f gyroVector = new Vector3f(); - public final Vector3f accelVector = new Vector3f(); + //public final Vector3f gyroVector = new Vector3f(); + //public final Vector3f accelVector = new Vector3f(); public final Vector3f magVector = new Vector3f(); public final Quaternion rotQuaternion = new Quaternion(); public final Quaternion rotMagQuaternion = new Quaternion(); - protected final Quaternion rotAdjust = new Quaternion(); + public final Quaternion rotAdjust = new Quaternion(); protected final Quaternion correction = new Quaternion(); protected TrackerMountingRotation mounting = null; protected TrackerStatus status = TrackerStatus.OK;