Gate yawConsistency on pose initialization to fix cold-start vision rejection

src/main/java/frc/robot/Robot.java

@@ -183,7 +183,8 @@ public Robot() {
         vision =
             new Vision(
                 drive::addVisionMeasurement,
-                drive::getFieldRelativeHeading,
+                drive::isPoseAsserted,
+                () -> 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::isPoseAsserted,
+                () -> 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::isPoseAsserted,
+                () -> drive.getPose().getRotation(),
                 new VisionIO() {},
                 new VisionIO() {},
                 new VisionIO() {},

src/main/java/frc/robot/subsystems/drive/Drive.java

@@ -78,8 +78,7 @@ public class Drive extends SubsystemBase {
       };
   private SwerveDrivePoseEstimator visionPose =
       new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d());
-  private boolean firstVisionEstimate = true;
-  private boolean poseInitialized = false;
+  private boolean poseAsserted = false;
 
   private static final ChassisSpeeds ZERO_SPEEDS = new ChassisSpeeds();
   private final SwerveModuleState[] emptyModuleStates = new SwerveModuleState[] {};
@@ -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.
+   * 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 isPoseAsserted() {
+    return poseAsserted;
   }
 
   public Rotation2d getRawGyroRotation() {
@@ -403,18 +402,19 @@ 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. */
   public void addVisionMeasurement(
       Pose2d visionRobotPoseMeters,
       double timestampSeconds,
       Matrix<N3, N1> 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 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);
     }
 
     visionPose.addVisionMeasurement(

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<Rotation2d> gyroYawSupplier;
+  private final BooleanSupplier poseAssertedSupplier;
+  private final Supplier<Rotation2d> headingSupplier;
   private final VisionIO[] io;
   private final VisionInputs[] visionInputs;
   private final VisionIOInputsAutoLogged[] inputs;
@@ -75,12 +77,21 @@ public class Vision extends SubsystemBase {
   // Cycle counter for throttled logging
   private int loopCounter = 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<Test> enabledTests = VisionFilter.DEFAULT_ENABLED_TESTS;
 
-  public Vision(VisionConsumer consumer, Supplier<Rotation2d> gyroYawSupplier, VisionIO... io) {
+  public Vision(
+      VisionConsumer consumer,
+      BooleanSupplier poseAssertedSupplier,
+      Supplier<Rotation2d> headingSupplier,
+      VisionIO... io) {
     this.consumer = consumer;
-    this.gyroYawSupplier = gyroYawSupplier;
+    this.poseAssertedSupplier = poseAssertedSupplier;
+    this.headingSupplier = headingSupplier;
     this.io = io;
 
     // Initialize per-camera velocity tracking arrays
@@ -177,7 +188,9 @@ public void periodic() {
                 cameraIndex,
                 lastAcceptedPose[cameraIndex],
                 lastAcceptedTimestamp[cameraIndex],
-                gyroYawSupplier.get(),
+                (poseAssertedSupplier.getAsBoolean() || headingCorroborated)
+                    ? headingSupplier.get()
+                    : null,
                 enabledTests);
 
         observationBuffer.add(tested);
@@ -241,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(),