Expose NT entry change time in PhotonLib (#562)

Adds target change timestamp to PhotonPipelineResult

Co-authored-by: Matt <matthew.morley.ca@gmail.com>

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

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

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<PhotonTrackedTarget, 10> targets;
   inline static bool HAS_WARNED = false;
 };

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 <code>
+     * getLatencyMillis()</code>
+     *
+     * @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.
      *

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(