asdf

PhotonVision · Mar 28, 2024 · 0a57b37 · 0a57b37
1 parent 5d80e8c
commit 0a57b37
Show file tree

Hide file tree

Showing 7 changed files with 25 additions and 17 deletions.
diff --git a/photon-lib/py/photonlibpy/photonCamera.py b/photon-lib/py/photonlibpy/photonCamera.py
@@ -78,6 +78,7 @@ def __init__(self, cameraName: str):
     def getLatestResult(self) -> PhotonPipelineResult:
         self._versionCheck()
 
+        now = RobotController.getFPGATime()
         retVal = PhotonPipelineResult()
         packetWithTimestamp = self._rawBytesEntry.getAtomic()
         byteList = packetWithTimestamp.value
@@ -89,7 +90,7 @@ def getLatestResult(self) -> PhotonPipelineResult:
             pkt = Packet(byteList)
             retVal.populateFromPacket(pkt)
             # We don't trust NT4 time, hack around
-            retVal.ntRecieveTimestampMicros = RobotController.getFPGATime()
+            retVal.ntRecieveTimestampMicros = now
             return retVal
 
     def getDriverMode(self) -> bool:

diff --git a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp
@@ -115,15 +115,15 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() {
   PhotonPipelineResult result;
 
   // Fill the packet with latest data and populate result.
+  units::microsecond_t now = frc::RobotController::GetFPGATime();
   const auto value = rawBytesEntry.Get();
   if (!value.size()) return result;
 
   photon::Packet packet{value};
 
   packet >> result;
 
-  result.SetTimestamp(units::microsecond_t(rawBytesEntry.GetLastChange()) -
-                      result.GetLatency());
+  result.SetRecieveTimestamp(now);
 
   return result;
 }

diff --git a/...n-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java b/...n-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java
@@ -49,7 +49,7 @@ public ProtobufPhotonPipelineResult createMessage() {
     @Override
     public PhotonPipelineResult unpack(ProtobufPhotonPipelineResult msg) {
         return new PhotonPipelineResult(
-                msg.getSequenceID(),
+                msg.getSequenceId(),
                 msg.getCaptureTimestampMicros(),
                 msg.getNtPublishTimestampMicros(),
                 PhotonTrackedTarget.proto.unpack(msg.getTargets()),
@@ -61,7 +61,7 @@ public void pack(ProtobufPhotonPipelineResult msg, PhotonPipelineResult value) {
         PhotonTrackedTarget.proto.pack(msg.getMutableTargets(), value.getTargets());
         MultiTargetPNPResult.proto.pack(msg.getMutableMultiTargetResult(), value.getMultiTagResult());
 
-        msg.setSequenceID(value.getSequenceID());
+        msg.setSequenceId(value.getSequenceID());
         msg.setCaptureTimestampMicros(value.getCaptureTimestampMicros());
         msg.setNtPublishTimestampMicros(value.getPublishTimestampMicros());
     }

diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp
@@ -74,8 +74,11 @@ Packet& operator>>(Packet& packet, PhotonPipelineResult& result) {
 
   packet >> multitagResult;
 
-  return PhotonPipelineResult{
-      sequenceID, units::microsecond_t(capTS) units::microsecond_t(pubTS),
+  units::microsecond_t captureTS = units::microsecond_t{static_cast<double>(capTS)};
+  units::microsecond_t publishTS = units::microsecond_t{static_cast<double>(pubTS)};
+
+  result = PhotonPipelineResult{
+      sequenceID, captureTS, publishTS,
       targets, multitagResult};
 
   return packet;

diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/proto/PhotonPipelineResultProto.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/proto/PhotonPipelineResultProto.cpp
@@ -41,7 +41,10 @@ wpi::Protobuf<photon::PhotonPipelineResult>::Unpack(
   }
 
   return photon::PhotonPipelineResult{
-      units::millisecond_t{m->latency_ms()}, targets,
+      m->sequence_id(),
+      units::microsecond_t{static_cast<double>(m->capture_timestamp_micros())},
+      units::microsecond_t{static_cast<double>(m->nt_publish_timestamp_micros())},
+      targets,
       wpi::UnpackProtobuf<photon::MultiTargetPNPResult>(
           m->multi_target_result())};
 }
@@ -50,7 +53,9 @@ void wpi::Protobuf<photon::PhotonPipelineResult>::Pack(
     google::protobuf::Message* msg, const photon::PhotonPipelineResult& value) {
   auto m = static_cast<photonvision::proto::ProtobufPhotonPipelineResult*>(msg);
 
-  m->set_latency_ms(value.latency.value());
+  m->set_sequence_id(value.sequenceID);
+  m->set_capture_timestamp_micros(value.captureTimestamp.value());
+  m->set_nt_publish_timestamp_micros(value.publishTimestamp.value());
 
   m->clear_targets();
   for (const auto& t : value.GetTargets()) {

diff --git a/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h b/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h
@@ -50,9 +50,8 @@ class PhotonPipelineResult {
    * @param targets The list of targets identified by the pipeline.
    * @param multitagResult The multitarget result. Default to empty
    */
-  PhotonPipelineResult(int64_t sequenceID, units
-                       : microsecond_t captureTimestamp, units
-                       : microsecond_t publishTimestamp,
+  PhotonPipelineResult(int64_t sequenceID, units::microsecond_t captureTimestamp, 
+                        units::microsecond_t publishTimestamp,
                          std::span<const PhotonTrackedTarget> targets,
                          MultiTargetPNPResult multitagResult = {});
 
@@ -108,8 +107,8 @@ class PhotonPipelineResult {
   const int64_t SequenceID() const { return sequenceID; }
 
   /** Sets the FPGA timestamp this result was recieved by robot code */
-  void SetRecieveTimestampMicros(const units::second_t timestamp) {
-    this->ntRecieveTimestampMicros = timestamp;
+  void SetRecieveTimestamp(const units::second_t timestamp) {
+    this->ntRecieveTimestamp = timestamp;
   }
 
   /**

diff --git a/photon-targeting/src/main/proto/photon.proto b/photon-targeting/src/main/proto/photon.proto
@@ -61,7 +61,7 @@ message ProtobufPhotonPipelineResult {
   repeated ProtobufPhotonTrackedTarget targets = 2;
   ProtobufMultiTargetPNPResult multi_target_result = 3;
 
-  int64 sequenceID = 4;
-  int64 captureTimestampMicros = 5;
-  int64 ntPublishTimestampMicros = 6;
+  int64 sequence_id = 4;
+  int64 capture_timestamp_micros = 5;
+  int64 nt_publish_timestamp_micros = 6;
 }