From 203e87d0623692f381c1d12568fda4e1b577b8e0 Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Sun, 27 Jul 2025 07:07:37 -0700 Subject: [PATCH] Add an overload of resetHeadingData() which takes in a Rotation3d --- .../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 fc61fe91a8..6cd76c7e06 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 void setLastPose(Pose2d lastPose) { * 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 void addHeadingData(double timestampSeconds, Rotation3d heading) { * 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 void addHeadingData(double timestampSeconds, Rotation2d 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 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. */