Use ReadQueue for PhotonCamera timestamps (#1316)

This removes the extra GetLastChange call to keep everything properly
atomic.

Closes #1303
This commit is contained in:
Matt
2024-08-04 14:23:46 -04:00
committed by GitHub
parent 37e9d40762
commit 67463a020a
29 changed files with 1057 additions and 1614 deletions

View File

@@ -1,4 +1,5 @@
from enum import Enum
from typing import List
import ntcore
from wpilib import RobotController, Timer
import wpilib
@@ -75,6 +76,39 @@ class PhotonCamera:
self._prevHeartbeat = 0
self._prevHeartbeatChangeTime = Timer.getFPGATimestamp()
def getAllUnreadResults(self) -> List[PhotonPipelineResult]:
"""
The list of pipeline results sent by PhotonVision since the last call to getAllUnreadResults().
Calling this function clears the internal FIFO queue, and multiple calls to
getAllUnreadResults() will return different (potentially empty) result arrays. Be careful to
call this exactly ONCE per loop of your robot code! FIFO depth is limited to 20 changes, so
make sure to call this frequently enough to avoid old results being discarded, too!
"""
self._versionCheck()
changes = self._rawBytesEntry.readQueue()
ret = []
for change in changes:
byteList = change.value
timestamp = change.time
if len(byteList) < 1:
pass
else:
newResult = PhotonPipelineResult()
pkt = Packet(byteList)
newResult.populateFromPacket(pkt)
# NT4 allows us to correct the timestamp based on when the message was sent
newResult.setTimestampSeconds(
timestamp / 1e6 - newResult.getLatencyMillis() / 1e3
)
ret.append(newResult)
return ret
def getLatestResult(self) -> PhotonPipelineResult:
self._versionCheck()