Package frc.robot.subsystems.swerve.util

Class PhoenixOdometryThread

java.lang.Object
java.lang.Thread
frc.robot.subsystems.swerve.util.PhoenixOdometryThread
All Implemented Interfaces:
Runnable
@NullMarked public class PhoenixOdometryThread extends Thread
Background thread responsible for sampling odometry-related signals at a higher frequency than the main robot loop.

This thread is primarily intended to reduce latency and aliasing when collecting drivetrain sensor data (e.g., wheel encoders, gyro rates) used for odometry and state estimation.

Two categories of signals may be registered:

  • Phoenix signals - StatusSignal instances that can be synchronized and timestamped by the CTRE Phoenix framework
  • Generic signals - arbitrary DoubleSuppliers sampled at the same rate as Phoenix signals

Each registered signal is associated with a thread-safe Queue into which samples are pushed. Consumers are expected to poll these queues from the main loop while holding the provided odometry lock.

If running on a CAN-FD network, Phoenix signals are synchronized using BaseStatusSignal.waitForAll(double, BaseStatusSignal...), allowing all signals to be sampled at nearly the same timestamp. On non-CAN-FD networks, the thread sleeps for the configured odometry period and explicitly refreshes signals.

The thread runs as a daemon and will terminate automatically when the JVM exits.

  • Constructor Details

    • PhoenixOdometryThread

      public PhoenixOdometryThread(Lock odometryLock)
      Creates a new odometry sampling thread.
      Parameters:
      odometryLock - lock shared with consumers to ensure that sampled values and timestamps are read atomically

  • Method Details

    • start

      public void start()
      Starts the thread if at least one signal has been registered.

      This prevents spawning an idle background thread when no signals are being sampled.

      Overrides:
      start in class Thread

    • registerSignal

      public Queue<Double> registerSignal(com.ctre.phoenix6.StatusSignal<?> signal)
      Registers a Phoenix StatusSignal to be sampled by the thread.

      The returned queue will receive one value per odometry tick.

      This method is thread-safe and may be called before the thread starts.

      Parameters:
      signal - Phoenix status signal to sample
      Returns:
      queue containing sampled signal values

    • registerSignal

      public Queue<Double> registerSignal(DoubleSupplier signal)
      Registers a generic value supplier to be sampled at the odometry rate.

      This is useful for signals not managed by Phoenix (e.g., WPILib sensors or computed values).

      Parameters:
      signal - supplier producing the value to sample
      Returns:
      queue containing sampled values

    • makeTimestampQueue

      public Queue<Double> makeTimestampQueue()
      Creates a queue that receives timestamps corresponding to each sampling cycle.

      The timestamp represents FPGA time in seconds, adjusted for average Phoenix signal latency when applicable.

      Returns:
      queue of sample timestamps (seconds)

    • run

      public void run()
      Main sampling loop.

      The thread runs indefinitely, sampling all registered signals at Constants.Swerve.odometryFrequency.

      Sampling behavior differs depending on CAN capabilities:

      • CAN-FD: waits for all Phoenix signals to update together
      • Non-CAN-FD: sleeps for the target period and manually refreshes

      All sampled values and timestamps are enqueued atomically under the odometry lock to ensure consistency for consumers.

      Specified by:
      run in interface Runnable
      Overrides:
      run in class Thread