diff --git a/photon-lib/py/photonlibpy/packet.py b/photon-lib/py/photonlibpy/packet.py index 559134091e..22d407479c 100644 --- a/photon-lib/py/photonlibpy/packet.py +++ b/photon-lib/py/photonlibpy/packet.py @@ -96,6 +96,14 @@ def decode32(self) -> int: """ return self._decodeGeneric(">l", 4) + def decodei64(self) -> int: + """ + * Returns a decoded int64 from the packet. + * + * @return A decoded int64 from the packet. + """ + return self._decodeGeneric(">q", 8) + def decodeDouble(self) -> float: """ * Returns a decoded double from the packet. diff --git a/photon-lib/py/photonlibpy/photonCamera.py b/photon-lib/py/photonlibpy/photonCamera.py index 8be306aa5b..99c12eb6d2 100644 --- a/photon-lib/py/photonlibpy/photonCamera.py +++ b/photon-lib/py/photonlibpy/photonCamera.py @@ -1,6 +1,6 @@ from enum import Enum import ntcore -from wpilib import Timer +from wpilib import RobotController, Timer import wpilib from photonlibpy.packet import Packet from photonlibpy.photonPipelineResult import PhotonPipelineResult @@ -88,10 +88,8 @@ def getLatestResult(self) -> PhotonPipelineResult: else: pkt = Packet(byteList) retVal.populateFromPacket(pkt) - # NT4 allows us to correct the timestamp based on when the message was sent - retVal.setTimestampSeconds( - timestamp / 1e6 - retVal.getLatencyMillis() / 1e3 - ) + # We don't trust NT4 time, hack around + retVal.ntRecieveTimestampMicros = RobotController.getFPGATime() return retVal def getDriverMode(self) -> bool: diff --git a/photon-lib/py/photonlibpy/photonPipelineResult.py b/photon-lib/py/photonlibpy/photonPipelineResult.py index fb045546fa..0895bfc67f 100644 --- a/photon-lib/py/photonlibpy/photonPipelineResult.py +++ b/photon-lib/py/photonlibpy/photonPipelineResult.py @@ -7,14 +7,27 @@ @dataclass class PhotonPipelineResult: - latencyMillis: float = -1.0 - timestampSec: float = -1.0 + # Image capture and NT publish timestamp, in microseconds and in the coprocessor timebase. As + # reported by WPIUtilJNI::now. + captureTimestampMicros: int = -1 + publishTimestampMicros: int = -1 + + # Mirror of the heartbeat entry -- monotonically increasing + sequenceID: int = -1 + + # Since we don't trust NT time sync, keep track of when we got this packet into robot code + ntRecieveTimestampMicros: int = -1 + targets: list[PhotonTrackedTarget] = field(default_factory=list) multiTagResult: MultiTargetPNPResult = field(default_factory=MultiTargetPNPResult) def populateFromPacket(self, packet: Packet) -> Packet: self.targets = [] - self.latencyMillis = packet.decodeDouble() + + self.sequenceID = packet.decodei64() + self.captureTimestampMicros = packet.decodei64() + self.publishTimestampMicros = packet.decodei64() + targetCount = packet.decode8() for _ in range(targetCount): @@ -27,14 +40,19 @@ def populateFromPacket(self, packet: Packet) -> Packet: return packet - def setTimestampSeconds(self, timestampSec: float) -> None: - self.timestampSec = timestampSec - def getLatencyMillis(self) -> float: - return self.latencyMillis - - def getTimestamp(self) -> float: - return self.timestampSec + return (self.publishTimestampMicros - self.captureTimestampMicros) / 1e3 + + def getTimestampSeconds(self) -> float: + """ + Returns the estimated time the frame was taken, in the recieved system's time base. This is + calculated as (NT recieve time (robot base) - (publish timestamp, coproc timebase - capture + timestamp, coproc timebase)) + """ + return ( + self.ntRecieveTimestampMicros + - (self.publishTimestampMicros - self.captureTimestampMicros) + ) / 1e6 def getTargets(self) -> list[PhotonTrackedTarget]: return self.targets diff --git a/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h b/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h index c0965fbd9c..4712531ef6 100644 --- a/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h +++ b/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h @@ -49,7 +49,11 @@ 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 */ @@ -125,8 +129,16 @@ 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; + // 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; + + // Mirror of the heartbeat entry -- monotonically increasing + units::microsecond_t sequenceID = -1; + wpi::SmallVector targets; MultiTargetPNPResult multitagResult; inline static bool HAS_WARNED = false;