diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index ad83ceb32..05e3fdb53 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -102,9 +102,14 @@ public class PhotonCamera { // Populate packet and create result. packet.setData(rawBytesEntry.getRaw(new byte[] {})); + if (packet.getSize() < 1) return ret; ret.createFromPacket(packet); + // Set the timestamp of the result. + // getLatestChange returns in microseconds so we divide by 1e6 to convert to seconds. + ret.setTimestampSeconds((rawBytesEntry.getLastChange() / 1e6) - ret.getLatencyMillis() / 1e3); + // Return result. return ret; } diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp index 1af6f0ed9..dcae3b198 100644 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp @@ -72,6 +72,10 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() { photonlib::Packet packet{bytes}; packet >> result; + + result.SetTimestamp(units::microsecond_t(rawBytesEntry.GetLastChange()) - + result.GetLatency()); + return result; } diff --git a/photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h b/photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h index dd7bc5285..572f430e9 100644 --- a/photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h +++ b/photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h @@ -79,6 +79,22 @@ class PhotonPipelineResult { */ units::second_t GetLatency() const { return latency; } + /** + * Returns the estimated time the frame was taken, + * This is much more accurate than using GetLatency() + * @return The timestamp in seconds or -1 if this result was not initiated + * with a timestamp. + */ + units::second_t GetTimestamp() const { return timestamp; } + + /** + * Sets the timestamp in seconds + * @param timestamp The timestamp in seconds + */ + void SetTimestamp(const units::second_t timestamp) { + this->timestamp = timestamp; + } + /** * Returns whether the pipeline has targets. * @return Whether the pipeline has targets. @@ -101,6 +117,7 @@ class PhotonPipelineResult { private: units::second_t latency = 0_s; + units::second_t timestamp = -1_s; wpi::SmallVector targets; inline static bool HAS_WARNED = false; }; diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java index 89665226c..b4b82a58e 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -32,6 +32,9 @@ public class PhotonPipelineResult { // Latency in milliseconds. private double latencyMillis; + // Timestamp in milliseconds. + private double timestampSeconds = -1; + /** Constructs an empty pipeline result. */ public PhotonPipelineResult() {} @@ -83,6 +86,25 @@ public class PhotonPipelineResult { return latencyMillis; } + /** + * Returns the estimated time the frame was taken, This is more accurate than using + * getLatencyMillis() + * + * @return The timestamp in seconds, or -1 if this result has no timestamp set. + */ + public double getTimestampSeconds() { + return timestampSeconds; + } + + /** + * Sets the FPGA timestamp of this result in seconds. + * + * @param timestampSeconds The timestamp in seconds. + */ + public void setTimestampSeconds(double timestampSeconds) { + this.timestampSeconds = timestampSeconds; + } + /** * Returns whether the pipeline has targets. * 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 3df5eb8ac..52c95c2f5 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 @@ -34,7 +34,6 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.numbers.N5; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.AnalogGyro; -import edu.wpi.first.wpilibj.Timer; import org.photonvision.PhotonCamera; /** @@ -84,7 +83,7 @@ public class DrivetrainPoseEstimator { var res = cam.getLatestResult(); if (res.hasTargets()) { - var imageCaptureTime = Timer.getFPGATimestamp() - res.getLatencyMillis() / 1000.0; + var imageCaptureTime = res.getTimestampSeconds(); var camToTargetTrans = res.getBestTarget().getBestCameraToTarget(); var camPose = Constants.kFarTargetPose.transformBy(camToTargetTrans.inverse()); m_poseEstimator.addVisionMeasurement(