using updated pose estimator setup

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

@@ -29,14 +29,14 @@
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
-import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.DriveConstants;
 import frc.robot.subsystems.drive.module.Module;
 import frc.robot.subsystems.drive.module.ModuleIO;
+import frc.robot.subsystems.drive.module.PhoenixOdometryThread;
 import frc.robot.subsystems.vision.VisionSubsystem;
 
 import java.util.concurrent.locks.Lock;
@@ -61,7 +61,13 @@ public class DriveSubsystem extends SubsystemBase {
   private final Field2d m_field = new Field2d();
 
   private Pose2d pose = new Pose2d();
-  private Rotation2d lastGyroRotation = new Rotation2d();
+  private Rotation2d m_rawGyroRotation = new Rotation2d();
+  private SwerveModulePosition[] m_lastModulePositions = new SwerveModulePosition[] {
+      new SwerveModulePosition(),
+      new SwerveModulePosition(),
+      new SwerveModulePosition(),
+      new SwerveModulePosition()
+  };
 
   private final VisionSubsystem[] m_cameras;
 
@@ -117,6 +123,8 @@ public DriveSubsystem(
     modules[2] = new Module(blModuleIO);
     modules[3] = new Module(brModuleIO);
 
+    PhoenixOdometryThread.getInstance().start();
+
     m_thetaPid = new PIDController(0.0, 0.0, 0.0);
     m_thetaPid.enableContinuousInput(-Math.PI, Math.PI);
     m_thetaPidProperty = new WpiPidPropertyBuilder("Drive/Theta Alignment", false, m_thetaPid)
@@ -179,37 +187,37 @@ public void periodic() {
     }
 
     // Update odometry
-//    int deltaCount =
-//        gyroInputs.isConnected() ? gyroInputs.getOdometryYawPositions().length : Integer.MAX_VALUE;
-//    for (int i = 0; i < 4; i++) {
-//      deltaCount = Math.min(deltaCount, modules[i].getPositionDeltas().length);
-//    }
-//    Logger.recordOutput("Odometry/Delta Count", deltaCount);
-//    for (int deltaIndex = 0; deltaIndex < deltaCount; deltaIndex++) {
-//       Read wheel deltas from each module
-//      SwerveModulePosition[] wheelDeltas = new SwerveModulePosition[4];
-//      for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) {
-//        wheelDeltas[moduleIndex] = modules[moduleIndex].getPositionDeltas()[deltaIndex];
-//      }
-
-      // The twist represents the motion of the robot since the last
-      // sample in x, y, and theta based only on the modules, without
-      // the gyro. The gyro is always disconnected in simulation.
-//      var twist = kinematics.toTwist2d(wheelDeltas);
-//      if (gyroInputs.isConnected()) {
-        // If the gyro is connected, replace the theta component of the twist
-        // with the change in angle since the last sample.
-//        Rotation2d gyroRotation = gyroInputs.getOdometryYawPositions()[deltaIndex];
-//        twist = new Twist2d(twist.dx, twist.dy, gyroRotation.minus(lastGyroRotation).getRadians());
-//        lastGyroRotation = gyroRotation;
-//      }
-//      Logger.recordOutput("Odometry/Twist", twist);
-      // Apply the twist (change since last sample) to the current pose
-//      m_poseEstimator.addDriveData(Timer.getFPGATimestamp(), twist);
-//      pose = pose.exp(twist);
-//    }
-
-//    List<PoseEstimator.TimestampedVisionUpdate> visionUpdates = new java.util.ArrayList<>(List.of());
+    double[] sampleTimestamps =
+        modules[0].getOdometryTimestamps(); // All signals are sampled together
+    int sampleCount = sampleTimestamps.length;
+    for (int i = 0; i < sampleCount; i++) {
+      // Read wheel positions and deltas from each module
+      SwerveModulePosition[] modulePositions = new SwerveModulePosition[4];
+      SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4];
+      for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) {
+        modulePositions[moduleIndex] = modules[moduleIndex].getOdometryPositions()[i];
+        moduleDeltas[moduleIndex] =
+            new SwerveModulePosition(
+                modulePositions[moduleIndex].distanceMeters
+                    - m_lastModulePositions[moduleIndex].distanceMeters,
+                modulePositions[moduleIndex].angle);
+        m_lastModulePositions[moduleIndex] = modulePositions[moduleIndex];
+      }
+
+      // Update gyro angle
+      if (gyroInputs.connected) {
+        // Use the real gyro angle
+        m_rawGyroRotation = gyroInputs.odometryYawPositions[i];
+      } else {
+        // Use the angle delta from the kinematics and module deltas
+        Twist2d twist = kinematics.toTwist2d(moduleDeltas);
+        m_rawGyroRotation = m_rawGyroRotation.plus(new Rotation2d(twist.dtheta));
+      }
+
+      // Apply update
+      m_wpiPoseEstimator.updateWithTime(sampleTimestamps[i], m_rawGyroRotation, modulePositions);
+    }
+
     for (VisionSubsystem camera : m_cameras) {
       camera.updateInputs();
       camera.getPose(m_wpiPoseEstimator.getEstimatedPosition()).ifPresent(
@@ -218,8 +226,6 @@ public void periodic() {
       );
     }
 
-//    m_poseEstimator.addVisionData(visionUpdates);
-
     m_wpiPoseEstimator.update(gyroInputs.yawPosition, getModulePositions());
     m_thetaPidProperty.updateIfChanged();
 

src/main/java/frc/robot/subsystems/drive/module/Module.java

@@ -31,8 +31,7 @@ public class Module {
   private Rotation2d m_angleSetpoint = null; // Setpoint for closed loop control, null for open loop
   private Double m_speedSetpoint = null; // Setpoint for closed loop control, null for open loop
   private Rotation2d m_turnRelativeOffset = null; // Relative + Offset = Absolute
-  private double m_lastPositionMeters = 0.0; // Used for delta calculation
-  private SwerveModulePosition[] m_positionDeltas = new SwerveModulePosition[] {};
+  private SwerveModulePosition[] m_odometryPositions = new SwerveModulePosition[] {};
 
   public Module(ModuleIO io) {
     this.m_io = io;
@@ -77,20 +76,28 @@ public void periodic() {
       }
     }
 
-    // Calculate position deltas for odometry
-    int deltaCount =
-        Math.min(m_inputs.getOdometryDrivePositionsRad().length, m_inputs.getOdometryTurnPositions().length);
-    m_positionDeltas = new SwerveModulePosition[deltaCount];
-    for (int i = 0; i < deltaCount; i++) {
-      double positionMeters = m_inputs.getOdometryDrivePositionsRad()[i] * WHEEL_RADIUS;
+    // Calculate positions for odometry
+    int sampleCount = m_inputs.odometryTimestamps.length; // All signals are sampled together
+    m_odometryPositions = new SwerveModulePosition[sampleCount];
+    for (int i = 0; i < sampleCount; i++) {
+      double positionMeters = m_inputs.odometryDrivePositionsRad[i] * WHEEL_RADIUS;
       Rotation2d angle =
-          m_inputs.getOdometryTurnPositions()[i].plus(
+          m_inputs.odometryTurnPositions[i].plus(
               m_turnRelativeOffset != null ? m_turnRelativeOffset : new Rotation2d());
-      m_positionDeltas[i] = new SwerveModulePosition(positionMeters - m_lastPositionMeters, angle);
-      m_lastPositionMeters = positionMeters;
+      m_odometryPositions[i] = new SwerveModulePosition(positionMeters, angle);
     }
   }
 
+  /** Returns the module positions received this cycle. */
+  public SwerveModulePosition[] getOdometryPositions() {
+    return m_odometryPositions;
+  }
+
+  /** Returns the timestamps of the samples received this cycle. */
+  public double[] getOdometryTimestamps() {
+    return m_inputs.odometryTimestamps;
+  }
+
   /** Runs the module with the specified setpoint state. Returns the optimized state. */
   public SwerveModuleState runSetpoint(SwerveModuleState state) {
     // Optimize state based on current angle
@@ -159,11 +166,6 @@ public SwerveModuleState getState() {
     return new SwerveModuleState(getVelocityMetersPerSec(), getAngle());
   }
 
-  /** Returns the module position deltas received this cycle. */
-  public SwerveModulePosition[] getPositionDeltas() {
-    return m_positionDeltas;
-  }
-
   /** Returns the drive velocity in radians/sec. */
   public double getCharacterizationVelocity() {
     return m_inputs.getDriveVelocityRadPerSec();

src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java

@@ -31,6 +31,8 @@ class ModuleIOInputs {
     protected double[] odometryDrivePositionsRad = new double[] {};
     protected Rotation2d[] odometryTurnPositions = new Rotation2d[] {};
 
+    protected double[] odometryTimestamps = new double[] {};
+
     public double getDrivePositionRad() {
       return drivePositionRad;
     }

src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java

@@ -63,6 +63,7 @@ public class ModuleIOTalonFX implements ModuleIO {
   private final StatusSignal<Double> m_turnAbsolutePosition;
   private final StatusSignal<Double> m_turnPosition;
   private final Queue<Double> m_turnPositionQueue;
+  private final Queue<Double> timestampQueue;
   private final StatusSignal<Double> m_turnVelocity;
   private final StatusSignal<Double> m_turnAppliedVolts;
   private final StatusSignal<Double> m_turnCurrent;
@@ -166,6 +167,8 @@ public ModuleIOTalonFX(ModuleConstants moduleConstants) {
     m_turnClosedLoopError = m_turnTalon.getClosedLoopError();
     m_turnClosedLoopReference = m_turnTalon.getClosedLoopReference();
 
+    timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue();
+
     // setup refresh rates on all inputs
     BaseStatusSignal.setUpdateFrequencyForAll(
         Module.ODOMETRY_FREQUENCY, m_drivePosition, m_turnPosition);

src/main/java/frc/robot/subsystems/drive/module/PhoenixOdometryThread.java

@@ -17,16 +17,17 @@
 import com.ctre.phoenix6.CANBus;
 import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.hardware.ParentDevice;
-import edu.wpi.first.wpilibj.DriverStation;
-import frc.robot.subsystems.drive.DriveSubsystem;
-
 import java.util.ArrayList;
 import java.util.List;
 import java.util.Queue;
 import java.util.concurrent.ArrayBlockingQueue;
 import java.util.concurrent.locks.Lock;
 import java.util.concurrent.locks.ReentrantLock;
 
+import edu.wpi.first.wpilibj.Timer;
+import frc.robot.subsystems.drive.DriveSubsystem;
+//import org.littletonrobotics.junction.Logger;
+
 /**
  * Provides an interface for asynchronously reading high-frequency measurements to a set of queues.
  *
@@ -40,6 +41,7 @@ public class PhoenixOdometryThread extends Thread {
       new ReentrantLock(); // Prevents conflicts when registering signals
   private BaseStatusSignal[] signals = new BaseStatusSignal[0];
   private final List<Queue<Double>> queues = new ArrayList<>();
+  private final List<Queue<Double>> timestampQueues = new ArrayList<>();
   private boolean isCANFD = false;
 
   private static PhoenixOdometryThread instance = null;
@@ -54,7 +56,13 @@ public static PhoenixOdometryThread getInstance() {
   private PhoenixOdometryThread() {
     setName("PhoenixOdometryThread");
     setDaemon(true);
-    start();
+  }
+
+  @Override
+  public void start() {
+    if (timestampQueues.size() > 0) {
+      super.start();
+    }
   }
 
   public Queue<Double> registerSignal(ParentDevice device, StatusSignal<Double> signal) {
@@ -75,6 +83,17 @@ public Queue<Double> registerSignal(ParentDevice device, StatusSignal<Double> si
     return queue;
   }
 
+  public Queue<Double> makeTimestampQueue() {
+    Queue<Double> queue = new ArrayBlockingQueue<>(20);
+    DriveSubsystem.odometryLock.lock();
+    try {
+      timestampQueues.add(queue);
+    } finally {
+      DriveSubsystem.odometryLock.unlock();
+    }
+    return queue;
+  }
+
   @Override
   public void run() {
     while (true) {
@@ -89,26 +108,34 @@ public void run() {
           // of Pro licensing. No reasoning for this behavior
           // is provided by the documentation.
           Thread.sleep((long) (1000.0 / Module.ODOMETRY_FREQUENCY));
-          if (signals.length > 0) {
-            BaseStatusSignal.refreshAll(signals);
-          }
+          if (signals.length > 0) BaseStatusSignal.refreshAll(signals);
         }
       } catch (InterruptedException e) {
-        DriverStation.reportError(e.toString(), true);
-        Thread.currentThread().interrupt();
+        e.printStackTrace();
       } finally {
         signalsLock.unlock();
       }
 
       // Save new data to queues
       DriveSubsystem.odometryLock.lock();
       try {
+        double timestamp = Timer.getFPGATimestamp() / 1e6;
+        double totalLatency = 0.0;
+        for (BaseStatusSignal signal : signals) {
+          totalLatency += signal.getTimestamp().getLatency();
+        }
+        if (signals.length > 0) {
+          timestamp -= totalLatency / signals.length;
+        }
         for (int i = 0; i < signals.length; i++) {
           queues.get(i).offer(signals[i].getValueAsDouble());
         }
+        for (int i = 0; i < timestampQueues.size(); i++) {
+          timestampQueues.get(i).offer(timestamp);
+        }
       } finally {
         DriveSubsystem.odometryLock.unlock();
       }
     }
   }
-}
+}