From 7202348228f602b18da7ba04614285f71f35b6ce Mon Sep 17 00:00:00 2001 From: Andrew Gasser Date: Sun, 16 Oct 2022 19:46:57 -0500 Subject: [PATCH 1/3] Correct image capture time `Timer.getFPGATimesptamp()` returns the current time in _seconds_, but `res.getLatencyMillis()` is in _miliseconds_. --- .../examples/simposeest/robot/DrivetrainPoseEstimator.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/DrivetrainPoseEstimator.java b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/DrivetrainPoseEstimator.java index 15ebf57799..0b82d743a0 100644 --- a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/DrivetrainPoseEstimator.java +++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/DrivetrainPoseEstimator.java @@ -86,7 +86,7 @@ public void update( var res = cam.getLatestResult(); if (res.hasTargets()) { - double imageCaptureTime = Timer.getFPGATimestamp() - res.getLatencyMillis(); + double imageCaptureTime = Timer.getFPGATimestamp() - res.getLatencyMillis() - 1000; Transform3d camToTargetTrans = res.getBestTarget().getCameraToTarget(); var transform = new Transform2d( From 5fcc2435c545e15734cac1e1c3c9740cfa59452d Mon Sep 17 00:00:00 2001 From: Andrew Gasser Date: Sun, 16 Oct 2022 19:51:33 -0500 Subject: [PATCH 2/3] Correct image capture time (correctly) --- .../examples/simposeest/robot/DrivetrainPoseEstimator.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/DrivetrainPoseEstimator.java b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/DrivetrainPoseEstimator.java index 0b82d743a0..ad52c6f672 100644 --- a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/DrivetrainPoseEstimator.java +++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/DrivetrainPoseEstimator.java @@ -86,7 +86,7 @@ public void update( var res = cam.getLatestResult(); if (res.hasTargets()) { - double imageCaptureTime = Timer.getFPGATimestamp() - res.getLatencyMillis() - 1000; + double imageCaptureTime = Timer.getFPGATimestamp() - res.getLatencyMillis() / 1000d; Transform3d camToTargetTrans = res.getBestTarget().getCameraToTarget(); var transform = new Transform2d( From e1f1626a5984530a04bcbe2de42463f46bb9caf4 Mon Sep 17 00:00:00 2001 From: Andrew Gasser Date: Sun, 16 Oct 2022 20:13:33 -0500 Subject: [PATCH 3/3] Change double literal to not use suffix --- .../examples/simposeest/robot/DrivetrainPoseEstimator.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/DrivetrainPoseEstimator.java b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/DrivetrainPoseEstimator.java index ad52c6f672..c9e6480c49 100644 --- a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/DrivetrainPoseEstimator.java +++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/DrivetrainPoseEstimator.java @@ -86,7 +86,7 @@ public void update( var res = cam.getLatestResult(); if (res.hasTargets()) { - double imageCaptureTime = Timer.getFPGATimestamp() - res.getLatencyMillis() / 1000d; + double imageCaptureTime = Timer.getFPGATimestamp() - res.getLatencyMillis() / 1000.0; Transform3d camToTargetTrans = res.getBestTarget().getCameraToTarget(); var transform = new Transform2d(