From 9469b39a99fc858a069e01897a3f850ad1623201 Mon Sep 17 00:00:00 2001 From: Christopher Larrieu Date: Fri, 27 Mar 2026 00:50:50 -0400 Subject: [PATCH 1/4] Refactor Vision to separate pose initialization check from heading supplier --- src/main/java/frc/robot/Robot.java | 9 ++++++--- .../frc/robot/subsystems/drive/Drive.java | 20 +++++++++---------- .../frc/robot/subsystems/vision/Vision.java | 15 ++++++++++---- 3 files changed, 27 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a1801a1..2f6c2ee 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -183,7 +183,8 @@ public Robot() { vision = new Vision( drive::addVisionMeasurement, - drive::getFieldRelativeHeading, + drive::isPoseInitialized, + () -> drive.getPose().getRotation(), new VisionIOPhotonVision(cameraFrontRightName, robotToFrontRightCamera), new VisionIOPhotonVision(cameraFrontLeftName, robotToFrontLeftCamera), new VisionIOPhotonVision(cameraBackRightName, robotToBackRightCamera), @@ -224,7 +225,8 @@ public Robot() { vision = new Vision( drive::addVisionMeasurement, - drive::getFieldRelativeHeading, + drive::isPoseInitialized, + () -> drive.getPose().getRotation(), new VisionIOPhotonVisionSim( cameraFrontRightName, robotToFrontRightCamera, drive::getPose), new VisionIOPhotonVisionSim( @@ -271,7 +273,8 @@ public Robot() { vision = new Vision( drive::addVisionMeasurement, - drive::getFieldRelativeHeading, + drive::isPoseInitialized, + () -> drive.getPose().getRotation(), new VisionIO() {}, new VisionIO() {}, new VisionIO() {}, diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 61e1e1e..98ae56e 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -78,7 +78,6 @@ public class Drive extends SubsystemBase { }; private SwerveDrivePoseEstimator visionPose = new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d()); - private boolean firstVisionEstimate = true; private boolean poseInitialized = false; private static final ChassisSpeeds ZERO_SPEEDS = new ChassisSpeeds(); @@ -388,12 +387,12 @@ public Pose2d getPose() { } /** - * Returns the field-relative heading for vision yaw validation, or null if the pose has not yet - * been initialized by vision or an auto routine. Returning null causes the yawConsistency test to - * be skipped, avoiding false rejections before the heading has field-relative meaning. + * Returns whether the pose estimator has been authoritatively initialized (by an auto routine or + * FMS). Before initialization, the gyro heading is robot-relative and should not be used for + * field-relative validation such as the vision yawConsistency test. */ - public Rotation2d getFieldRelativeHeading() { - return poseInitialized ? getPose().getRotation() : null; + public boolean isPoseInitialized() { + return poseInitialized; } public Rotation2d getRawGyroRotation() { @@ -411,10 +410,11 @@ public void addVisionMeasurement( Pose2d visionRobotPoseMeters, double timestampSeconds, Matrix visionMeasurementStdDevs) { - // Initialize pose from the first vision estimate while disabled - if (firstVisionEstimate && RobotState.isDisabled()) { - setPose(visionRobotPoseMeters); - firstVisionEstimate = false; + // Track the latest vision estimate while disabled, before the pose is authoritatively + // initialized (by auto routine or FMS). Does not set poseInitialized, so yawConsistency + // stays dormant until the heading is known-good. + if (!poseInitialized && RobotState.isDisabled()) { + visionPose.resetPosition(rawGyroRotation, getModulePositions(), visionRobotPoseMeters); } visionPose.addVisionMeasurement( diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index c967e83..dee6673 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -31,12 +31,14 @@ import java.io.IOException; import java.util.ArrayList; import java.util.EnumSet; +import java.util.function.BooleanSupplier; import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; public class Vision extends SubsystemBase { private final VisionConsumer consumer; - private final Supplier gyroYawSupplier; + private final BooleanSupplier poseInitializedSupplier; + private final Supplier headingSupplier; private final VisionIO[] io; private final VisionInputs[] visionInputs; private final VisionIOInputsAutoLogged[] inputs; @@ -78,9 +80,14 @@ public class Vision extends SubsystemBase { // Vision tests to apply (remove from set to disable specific tests) public static final EnumSet enabledTests = VisionFilter.DEFAULT_ENABLED_TESTS; - public Vision(VisionConsumer consumer, Supplier gyroYawSupplier, VisionIO... io) { + public Vision( + VisionConsumer consumer, + BooleanSupplier poseInitializedSupplier, + Supplier headingSupplier, + VisionIO... io) { this.consumer = consumer; - this.gyroYawSupplier = gyroYawSupplier; + this.poseInitializedSupplier = poseInitializedSupplier; + this.headingSupplier = headingSupplier; this.io = io; // Initialize per-camera velocity tracking arrays @@ -177,7 +184,7 @@ public void periodic() { cameraIndex, lastAcceptedPose[cameraIndex], lastAcceptedTimestamp[cameraIndex], - gyroYawSupplier.get(), + poseInitializedSupplier.getAsBoolean() ? headingSupplier.get() : null, enabledTests); observationBuffer.add(tested); From 490e8056dd92b551f8d80245eea77d69842dc482 Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Fri, 27 Mar 2026 09:16:40 -0400 Subject: [PATCH 2/4] Remove inaccurate FMS reference from Drive.java comment FMS connection does not initialize the robot pose; only the auto routine does. Update comment to reflect this. Co-Authored-By: Claude Sonnet 4.6 --- src/main/java/frc/robot/subsystems/drive/Drive.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 98ae56e..7b391c5 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -411,7 +411,7 @@ public void addVisionMeasurement( double timestampSeconds, Matrix visionMeasurementStdDevs) { // Track the latest vision estimate while disabled, before the pose is authoritatively - // initialized (by auto routine or FMS). Does not set poseInitialized, so yawConsistency + // initialized (by auto routine). Does not set poseInitialized, so yawConsistency // stays dormant until the heading is known-good. if (!poseInitialized && RobotState.isDisabled()) { visionPose.resetPosition(rawGyroRotation, getModulePositions(), visionRobotPoseMeters); From 108084d4e5888efc6210cfd7dc8d94223531a169 Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Fri, 27 Mar 2026 09:54:17 -0400 Subject: [PATCH 3/4] Add vision-only yawConsistency bootstrap path via accepted-observation count MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Rename poseInitialized → poseAsserted throughout (Drive, Vision, Robot). Add visionAcceptedCount: yawConsistency now activates when either poseAsserted is true (set by auto) OR visionAcceptedCount reaches yawConsistencyMinAccepted (default 10), whichever comes first. This allows vision to self-bootstrap without an auto routine in practice/teleop-only scenarios. Log analysis of VACHE Q54 confirms 10 accepts arrive in ~1.9s once observations are in range. Co-Authored-By: Claude Sonnet 4.6 --- src/main/java/frc/robot/Robot.java | 6 +++--- .../java/frc/robot/subsystems/drive/Drive.java | 18 +++++++++--------- .../frc/robot/subsystems/vision/Vision.java | 17 +++++++++++++---- .../subsystems/vision/VisionConstants.java | 5 +++++ 4 files changed, 30 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2f6c2ee..8bbd756 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -183,7 +183,7 @@ public Robot() { vision = new Vision( drive::addVisionMeasurement, - drive::isPoseInitialized, + drive::isPoseAsserted, () -> drive.getPose().getRotation(), new VisionIOPhotonVision(cameraFrontRightName, robotToFrontRightCamera), new VisionIOPhotonVision(cameraFrontLeftName, robotToFrontLeftCamera), @@ -225,7 +225,7 @@ public Robot() { vision = new Vision( drive::addVisionMeasurement, - drive::isPoseInitialized, + drive::isPoseAsserted, () -> drive.getPose().getRotation(), new VisionIOPhotonVisionSim( cameraFrontRightName, robotToFrontRightCamera, drive::getPose), @@ -273,7 +273,7 @@ public Robot() { vision = new Vision( drive::addVisionMeasurement, - drive::isPoseInitialized, + drive::isPoseAsserted, () -> drive.getPose().getRotation(), new VisionIO() {}, new VisionIO() {}, diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 7b391c5..81c7e62 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -78,7 +78,7 @@ public class Drive extends SubsystemBase { }; private SwerveDrivePoseEstimator visionPose = new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d()); - private boolean poseInitialized = false; + private boolean poseAsserted = false; private static final ChassisSpeeds ZERO_SPEEDS = new ChassisSpeeds(); private final SwerveModuleState[] emptyModuleStates = new SwerveModuleState[] {}; @@ -387,12 +387,12 @@ public Pose2d getPose() { } /** - * Returns whether the pose estimator has been authoritatively initialized (by an auto routine or - * FMS). Before initialization, the gyro heading is robot-relative and should not be used for + * Returns whether the pose estimator has been authoritatively initialized by an auto routine. + * Before initialization, the gyro heading is robot-relative and should not be used for * field-relative validation such as the vision yawConsistency test. */ - public boolean isPoseInitialized() { - return poseInitialized; + public boolean isPoseAsserted() { + return poseAsserted; } public Rotation2d getRawGyroRotation() { @@ -402,7 +402,7 @@ public Rotation2d getRawGyroRotation() { /** Resets the pose estimator to the given pose. */ public void setPose(Pose2d pose) { visionPose.resetPosition(rawGyroRotation, getModulePositions(), pose); - poseInitialized = true; + poseAsserted = true; } /** Adds a new timestamped vision measurement. */ @@ -411,9 +411,9 @@ public void addVisionMeasurement( double timestampSeconds, Matrix visionMeasurementStdDevs) { // Track the latest vision estimate while disabled, before the pose is authoritatively - // initialized (by auto routine). Does not set poseInitialized, so yawConsistency - // stays dormant until the heading is known-good. - if (!poseInitialized && RobotState.isDisabled()) { + // initialized by an auto routine. Does not set poseAsserted; yawConsistency is gated + // in Vision.java on poseAsserted or accumulated accepted-observation count. + if (!poseAsserted && RobotState.isDisabled()) { visionPose.resetPosition(rawGyroRotation, getModulePositions(), visionRobotPoseMeters); } diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index dee6673..4849fef 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -37,7 +37,7 @@ public class Vision extends SubsystemBase { private final VisionConsumer consumer; - private final BooleanSupplier poseInitializedSupplier; + private final BooleanSupplier poseAssertedSupplier; private final Supplier headingSupplier; private final VisionIO[] io; private final VisionInputs[] visionInputs; @@ -77,16 +77,21 @@ public class Vision extends SubsystemBase { // Cycle counter for throttled logging private int loopCounter = 0; + // Running count of observations accepted without the yawConsistency check active. + // Once this reaches yawConsistencyMinAccepted, the heading is treated as field-relative + // and yawConsistency is enabled (vision-only path, independent of poseAsserted). + private int visionAcceptedCount = 0; + // Vision tests to apply (remove from set to disable specific tests) public static final EnumSet enabledTests = VisionFilter.DEFAULT_ENABLED_TESTS; public Vision( VisionConsumer consumer, - BooleanSupplier poseInitializedSupplier, + BooleanSupplier poseAssertedSupplier, Supplier headingSupplier, VisionIO... io) { this.consumer = consumer; - this.poseInitializedSupplier = poseInitializedSupplier; + this.poseAssertedSupplier = poseAssertedSupplier; this.headingSupplier = headingSupplier; this.io = io; @@ -184,7 +189,10 @@ public void periodic() { cameraIndex, lastAcceptedPose[cameraIndex], lastAcceptedTimestamp[cameraIndex], - poseInitializedSupplier.getAsBoolean() ? headingSupplier.get() : null, + (poseAssertedSupplier.getAsBoolean() + || visionAcceptedCount >= yawConsistencyMinAccepted) + ? headingSupplier.get() + : null, enabledTests); observationBuffer.add(tested); @@ -193,6 +201,7 @@ public void periodic() { robotPoses.add(observation.pose()); if (tested.score() > minScore) { robotPosesAccepted.add(observation.pose()); + visionAcceptedCount++; } else { robotPosesRejected.add(observation.pose()); } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index d54b8ac..495d4ab 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -137,6 +137,11 @@ public class VisionConstants { // 97%+ of legitimate observations. Multi-tag observations (0.9+) are unaffected. public static double minScore = 0.65; + // Number of accepted observations required before enabling the yawConsistency check via + // the vision-only path (independent of poseAsserted). After this many accepted poses, + // the heading is assumed to have enough field-relative meaning to validate new observations. + public static int yawConsistencyMinAccepted = 10; + // Feature flags public static boolean kLogIndividualCameraPoses = false; public static boolean kLogSummaryPoses = false; From 0c04841522810d3047bb870d68fe64b90797b2bd Mon Sep 17 00:00:00 2001 From: Christopher Larrieu Date: Fri, 27 Mar 2026 10:13:59 -0400 Subject: [PATCH 4/4] Trust heading after first multi-camera agreement. --- .../java/frc/robot/subsystems/vision/Vision.java | 16 +++++++++------- .../robot/subsystems/vision/VisionConstants.java | 5 ----- 2 files changed, 9 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 4849fef..eac51e4 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -77,10 +77,9 @@ public class Vision extends SubsystemBase { // Cycle counter for throttled logging private int loopCounter = 0; - // Running count of observations accepted without the yawConsistency check active. - // Once this reaches yawConsistencyMinAccepted, the heading is treated as field-relative - // and yawConsistency is enabled (vision-only path, independent of poseAsserted). - private int visionAcceptedCount = 0; + // Set true when multiple cameras independently agree on a pose, confirming the heading + // is field-relative. Enables yawConsistency without requiring an authoritative pose from auto. + private boolean headingCorroborated = false; // Vision tests to apply (remove from set to disable specific tests) public static final EnumSet enabledTests = VisionFilter.DEFAULT_ENABLED_TESTS; @@ -189,8 +188,7 @@ public void periodic() { cameraIndex, lastAcceptedPose[cameraIndex], lastAcceptedTimestamp[cameraIndex], - (poseAssertedSupplier.getAsBoolean() - || visionAcceptedCount >= yawConsistencyMinAccepted) + (poseAssertedSupplier.getAsBoolean() || headingCorroborated) ? headingSupplier.get() : null, enabledTests); @@ -201,7 +199,6 @@ public void periodic() { robotPoses.add(observation.pose()); if (tested.score() > minScore) { robotPosesAccepted.add(observation.pose()); - visionAcceptedCount++; } else { robotPosesRejected.add(observation.pose()); } @@ -257,6 +254,11 @@ public void periodic() { double linearStdDev = linearStdDevBaseline * cameraCountFactor / fused.score(); double angularStdDev = angularStdDevBaseline * cameraCountFactor / fused.score(); + // Multi-camera agreement corroborates the heading, enabling yawConsistency + if (fused.cameraCount() > 1) { + headingCorroborated = true; + } + // Send fused vision observation to the pose estimator consumer.accept( fused.pose(), diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 495d4ab..d54b8ac 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -137,11 +137,6 @@ public class VisionConstants { // 97%+ of legitimate observations. Multi-tag observations (0.9+) are unaffected. public static double minScore = 0.65; - // Number of accepted observations required before enabling the yawConsistency check via - // the vision-only path (independent of poseAsserted). After this many accepted poses, - // the heading is assumed to have enough field-relative meaning to validate new observations. - public static int yawConsistencyMinAccepted = 10; - // Feature flags public static boolean kLogIndividualCameraPoses = false; public static boolean kLogSummaryPoses = false;