From cefaa313df98cae15bf9712dd0b6a148b251bf4d Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Sun, 27 Jul 2025 17:28:16 -0700 Subject: [PATCH] Add an overload of resetHeadingData() which takes in a Rotation3d (#2013) --- .../org/photonvision/PhotonPoseEstimator.java | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index fc61fe91a..6cd76c7e0 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -316,7 +316,7 @@ public class PhotonPoseEstimator { * Add robot heading data to buffer. Must be called periodically for the * PNP_DISTANCE_TRIG_SOLVE strategy. * - * @param timestampSeconds timestamp of the robot heading data. + * @param timestampSeconds Timestamp of the robot heading data. * @param heading Field-relative robot heading at given timestamp. Standard WPILIB field * coordinates. */ @@ -328,7 +328,7 @@ public class PhotonPoseEstimator { * Add robot heading data to buffer. Must be called periodically for the * PNP_DISTANCE_TRIG_SOLVE strategy. * - * @param timestampSeconds timestamp of the robot heading data. + * @param timestampSeconds Timestamp of the robot heading data. * @param heading Field-relative robot heading at given timestamp. Standard WPILIB field * coordinates. */ @@ -340,7 +340,20 @@ public class PhotonPoseEstimator { * 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 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, Rotation3d heading) { + headingBuffer.clear(); + addHeadingData(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. */