From c1b0c8a831cc7984a1f8d6e8ec6f42b35e7a485f Mon Sep 17 00:00:00 2001 From: Bryce Roethel Date: Sat, 26 Apr 2025 00:57:27 -0400 Subject: [PATCH] [photon-lib] Add PhotonPoseEstimator.resetHeadingData() (#1933) --- .../java/org/photonvision/PhotonPoseEstimator.java | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index a389d5931..1675ad3d1 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -336,6 +336,19 @@ public class PhotonPoseEstimator { headingBuffer.addSample(timestampSeconds, heading); } + /** + * Clears all heading data in the buffer, and adds a new seed. Useful for preventing estimates + * from utilizing heading data provided prior to a pose or rotation reset. + * + * @param timestampSeconds timestamp of the robot heading data. + * @param heading Field-relative robot heading at given timestamp. Standard WPILIB field + * coordinates. + */ + public void resetHeadingData(double timestampSeconds, Rotation2d heading) { + headingBuffer.clear(); + addHeadingData(timestampSeconds, heading); + } + /** * @return The current transform from the center of the robot to the camera mount position */