diff --git a/chameleon-server/src/main/java/com/chameleonvision/util/MathHandler.java b/chameleon-server/src/main/java/com/chameleonvision/util/MathHandler.java index a95a089e3..f2bcbaa62 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/util/MathHandler.java +++ b/chameleon-server/src/main/java/com/chameleonvision/util/MathHandler.java @@ -24,4 +24,9 @@ public class MathHandler { public static double toSlope(Number angle){ return FastMath.atan(FastMath.toRadians(angle.doubleValue() - 90)); } + + public static double roundTo(double value, int to) { + double toMult = Math.pow(10, to); + return (double)Math.round(value * toMult) / toMult; + } } diff --git a/chameleon-server/src/main/java/com/chameleonvision/vision/VisionProcess.java b/chameleon-server/src/main/java/com/chameleonvision/vision/VisionProcess.java index af1c163f5..baad42628 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/vision/VisionProcess.java +++ b/chameleon-server/src/main/java/com/chameleonvision/vision/VisionProcess.java @@ -3,6 +3,7 @@ package com.chameleonvision.vision; import com.chameleonvision.Debug; import com.chameleonvision.config.ConfigManager; import com.chameleonvision.util.LoopingRunnable; +import com.chameleonvision.util.MathHandler; import com.chameleonvision.vision.camera.CameraStreamer; import com.chameleonvision.vision.camera.USBCameraCapture; import com.chameleonvision.vision.pipeline.*; @@ -47,7 +48,7 @@ public class VisionProcess { private NetworkTableEntry ntPitchEntry; private NetworkTableEntry ntAuxListEntry; private NetworkTableEntry ntAreaEntry; - private NetworkTableEntry ntTimeStampEntry; + private NetworkTableEntry ntLatencyEntry; private NetworkTableEntry ntValidEntry; private ObjectMapper objectMapper = new ObjectMapper(); @@ -108,7 +109,7 @@ public class VisionProcess { ntPitchEntry = newTable.getEntry("pitch"); ntYawEntry = newTable.getEntry("yaw"); ntAreaEntry = newTable.getEntry("area"); - ntTimeStampEntry = newTable.getEntry("timestamp"); + ntLatencyEntry = newTable.getEntry("latency"); ntValidEntry = newTable.getEntry("is_valid"); ntAuxListEntry = newTable.getEntry("aux_targets"); ntDriveModeListenerID = ntDriverModeEntry.addListener(this::setDriverMode, EntryListenerFlags.kUpdate); @@ -197,7 +198,7 @@ public class VisionProcess { //noinspection unchecked List targets = (List) data.targets; - ntTimeStampEntry.setDouble(data.imageTimestamp); + ntLatencyEntry.setDouble(MathHandler.roundTo(data.processTime * 1e-6, 3)); ntPitchEntry.setDouble(targets.get(0).pitch); ntYawEntry.setDouble(targets.get(0).yaw); ntAreaEntry.setDouble(targets.get(0).area); @@ -213,7 +214,7 @@ public class VisionProcess { ntPitchEntry.setDouble(0.0); ntYawEntry.setDouble(0.0); ntAreaEntry.setDouble(0.0); - ntTimeStampEntry.setDouble(0.0); + ntLatencyEntry.setDouble(0.0); ntAuxListEntry.setString(""); } }