Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-21 01:01:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Replace NT timestamp with latency

Browse Source
This commit is contained in:
Banks Troutman
2019-12-15 00:42:49 -05:00
parent 0fe8b6cb2b
commit 0446d3e546
2 changed files with 10 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
chameleon-server/src/main/java/com/chameleonvision/util/MathHandler.java
View File

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

9
chameleon-server/src/main/java/com/chameleonvision/vision/VisionProcess.java
View File

@@ -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<CVPipeline2d.Target2d> targets = (List<CVPipeline2d.Target2d>) 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("");
}
}