lol im dumb

src/main/java/frc/robot/subsystems/swerve/gyroIO/GyroInterface.java

@@ -10,14 +10,15 @@ public interface GyroInterface {
   public static class GyroInputs {
     public boolean isConnected = false;
 
-    public Rotation2d yawDegreesRotation2d = new Rotation2d();
     public double yawDegrees = 0.0;
+    public Rotation2d yawDegreesRotation2d = new Rotation2d();
     public double yawVelocity = 0.0;
     public double pitchDegrees = 0.0;
     public double rollDegrees = 0.0;
 
-    public Rotation2d[] odometryYawPositions = new Rotation2d[] {};
     public double[] accel = new double[] {0, 0, 0};
+
+    public Rotation2d[] odometryYawPositions = new Rotation2d[] {};
     public double[] odometryYawTimestamps = new double[] {};
   }
 

src/main/java/frc/robot/subsystems/swerve/gyroIO/PhysicalGyro.java

@@ -10,13 +10,15 @@
 import frc.robot.extras.util.AllianceFlipper;
 import frc.robot.subsystems.swerve.gyroIO.GyroInterface.GyroInputs;
 import frc.robot.subsystems.swerve.odometryThread.OdometryThread;
+import java.util.Queue;
 import java.util.function.Supplier;
 
 public class PhysicalGyro implements GyroInterface {
   private final AHRS gyro = new AHRS(SPI.Port.kMXP, (byte) 250);
+  private final Queue<Angle> yawPositionInput;
 
   public PhysicalGyro() {
-    OdometryThread.registerInput(getAngle());
+    yawPositionInput = OdometryThread.registerInput(getAngle());
   }
 
   @Override
@@ -27,12 +29,23 @@ public void updateInputs(GyroInputs inputs) {
     inputs.rollDegrees = getRoll().in(Degrees);
     inputs.pitchDegrees = getPitch().in(Degrees);
     inputs.yawDegrees = getYaw().in(Degrees);
+
+    // Handle odometry yaw positions
+    if (!yawPositionInput.isEmpty()) {
+      Rotation2d[] odometryYawPositions = new Rotation2d[yawPositionInput.size()];
+      int index = 0;
+      for (Angle angle : yawPositionInput) {
+        odometryYawPositions[index++] = Rotation2d.fromDegrees(angle.in(Degrees));
+      }
+      inputs.odometryYawPositions = odometryYawPositions;
+      yawPositionInput.clear();
+    }
   }
 
   public void zeroHeading() {
     gyro.reset();
   }
-  
+
   public Supplier<Angle> getAngle() {
     return () -> Degrees.of(-gyro.getAngle());
   }