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

Closes #1304

photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp

@@ -29,6 +29,7 @@
 #include <string_view>
 
 #include <frc/Errors.h>
+#include <frc/RobotController.h>
 #include <frc/Timer.h>
 #include <opencv2/core.hpp>
 #include <opencv2/core/mat.hpp>
@@ -115,6 +116,8 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() {
   PhotonPipelineResult result;
 
   // Fill the packet with latest data and populate result.
+  units::microsecond_t now =
+      units::microsecond_t(frc::RobotController::GetFPGATime());
   const auto value = rawBytesEntry.Get();
   if (!value.size()) return result;
 
@@ -122,8 +125,7 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() {
 
   packet >> result;
 
-  result.SetTimestamp(units::microsecond_t(rawBytesEntry.GetLastChange()) -
-                      result.GetLatency());
+  result.SetRecieveTimestamp(now);
 
   return result;
 }

photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h

@@ -40,6 +40,7 @@
 #include <utility>
 #include <vector>
 
+#include <frc/Timer.h>
 #include <frc/apriltag/AprilTagFieldLayout.h>
 #include <frc/apriltag/AprilTagFields.h>
 #include <units/math.h>
@@ -369,7 +370,10 @@ class PhotonCameraSim {
       multiTagResults = MultiTargetPNPResult{pnpResult, usedIds};
     }
 
-    return PhotonPipelineResult{latency, detectableTgts, multiTagResults};
+    units::second_t now = frc::Timer::GetFPGATimestamp();
+
+    return PhotonPipelineResult{heartbeatCounter, now - latency, now,
+                                detectableTgts, multiTagResults};
   }
   void SubmitProcessedFrame(const PhotonPipelineResult& result) {
     SubmitProcessedFrame(result, wpi::Now());
@@ -426,7 +430,7 @@ class PhotonCameraSim {
   PhotonCamera* cam;
 
   NTTopicSet ts{};
-  uint64_t heartbeatCounter{0};
+  int64_t heartbeatCounter{0};
 
   uint64_t nextNTEntryTime{wpi::Now()};
 

photon-lib/src/main/native/include/photon/simulation/SimPhotonCamera.h

@@ -90,7 +90,7 @@ class SimPhotonCamera : public PhotonCamera {
     latencyMillisEntry.SetDouble(latency.to<double>());
     std::sort(targetList.begin(), targetList.end(),
               [&](auto lhs, auto rhs) { return sortMode(lhs, rhs); });
-    PhotonPipelineResult newResult{latency, targetList};
+    PhotonPipelineResult newResult{0, 0_s, latency, targetList};
     Packet packet{};
     packet << newResult;