Add sequence ID, capture, publish and recieve timestamp to PhotonPipelineResult (#1305)

Closes #1304

photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h

@@ -41,21 +41,20 @@ class PhotonPipelineResult {
 
   /**
    * Constructs a pipeline result.
-   * @param latency The latency in the pipeline.
+   * @param sequenceID The number of frames processed by this camera since boot
+   * @param captureTimestamp The time, in uS in the coprocessor's timebase, that
+   * the coprocessor captured the image this result contains the targeting info
+   * of
+   * @param publishTimestamp The time, in uS in the coprocessor's timebase, that
+   * the coprocessor published targeting info
    * @param targets The list of targets identified by the pipeline.
+   * @param multitagResult The multitarget result. Default to empty
    */
-  PhotonPipelineResult(units::millisecond_t latency,
-                       std::span<const PhotonTrackedTarget> targets);
-
-  /**
-   * Constructs a pipeline result.
-   * @param latency The latency in the pipeline.
-   * @param targets The list of targets identified by the pipeline.
-   * @param multitagResult The multitarget result
-   */
-  PhotonPipelineResult(units::millisecond_t latency,
+  PhotonPipelineResult(int64_t sequenceID,
+                       units::microsecond_t captureTimestamp,
+                       units::microsecond_t publishTimestamp,
                        std::span<const PhotonTrackedTarget> targets,
-                       MultiTargetPNPResult multitagResult);
+                       MultiTargetPNPResult multitagResult = {});
 
   /**
    * Returns the best target in this pipeline result. If there are no targets,
@@ -81,7 +80,9 @@ class PhotonPipelineResult {
    * Returns the latency in the pipeline.
    * @return The latency in the pipeline.
    */
-  units::millisecond_t GetLatency() const { return latency; }
+  units::millisecond_t GetLatency() const {
+    return publishTimestamp - captureTimestamp;
+  }
 
   /**
    * Returns the estimated time the frame was taken,
@@ -89,7 +90,9 @@ class PhotonPipelineResult {
    * @return The timestamp in seconds or -1 if this result was not initiated
    * with a timestamp.
    */
-  units::second_t GetTimestamp() const { return timestamp; }
+  units::second_t GetTimestamp() const {
+    return ntRecieveTimestamp - (publishTimestamp - captureTimestamp);
+  }
 
   /**
    * Return the latest mulit-target result, as calculated on your coprocessor.
@@ -99,11 +102,14 @@ class PhotonPipelineResult {
   const MultiTargetPNPResult& MultiTagResult() const { return multitagResult; }
 
   /**
-   * Sets the timestamp in seconds
-   * @param timestamp The timestamp in seconds
+   * The number of non-empty frames processed by this camera since boot. Useful
+   * to checking if a camera is alive.
    */
-  void SetTimestamp(const units::second_t timestamp) {
-    this->timestamp = timestamp;
+  int64_t SequenceID() const { return sequenceID; }
+
+  /** Sets the FPGA timestamp this result was recieved by robot code */
+  void SetRecieveTimestamp(const units::second_t timestamp) {
+    this->ntRecieveTimestamp = timestamp;
   }
 
   /**
@@ -125,8 +131,17 @@ class PhotonPipelineResult {
   friend Packet& operator<<(Packet& packet, const PhotonPipelineResult& result);
   friend Packet& operator>>(Packet& packet, PhotonPipelineResult& result);
 
-  units::millisecond_t latency = 0_s;
-  units::second_t timestamp = -1_s;
+  // Mirror of the heartbeat entry -- monotonically increasing
+  int64_t sequenceID = -1;
+
+  // Image capture and NT publish timestamp, in microseconds and in the
+  // coprocessor timebase. As reported by WPIUtilJNI::now.
+  units::microsecond_t captureTimestamp;
+  units::microsecond_t publishTimestamp;
+  // Since we don't trust NT time sync, keep track of when we got this packet
+  // into robot code
+  units::microsecond_t ntRecieveTimestamp = -1_s;
+
   wpi::SmallVector<PhotonTrackedTarget, 10> targets;
   MultiTargetPNPResult multitagResult;
   inline static bool HAS_WARNED = false;