Package com.marslib.swerve

Class PhoenixOdometryThread

java.lang.Object

java.lang.Thread

com.marslib.swerve.PhoenixOdometryThread

All Implemented Interfaces:

Runnable

public class PhoenixOdometryThread
extends Thread

High-frequency CAN bus polling thread for CTRE Phoenix 6 odometry signals.

This singleton thread runs at a configurable odometry Hz (typically 250Hz), continuously sampling drive/turn encoder positions and gyro yaw from TalonFX and Pigeon2 devices. Samples are buffered in thread-safe BlockingQueues and drained by SwerveDrive.periodic() each robot loop iteration.

Thread Safety: All signal registration and data access is guarded by a ReentrantLock. The queues use a fixed capacity of 50 to bound memory usage.

Zero-Allocation Contract: All drain methods (getSyncData(int), getGyroYawData()) write into pre-allocated fixed-capacity arrays and return a valid sample count. No heap allocations occur on the hot path.

Nested Class Summary

Nested Classes

Modifier and Type	Class	Description
static class	PhoenixOdometryThread.GyroYawData	Container for pre-allocated gyro yaw data.
static class	PhoenixOdometryThread.SyncData	Container for a batch of synchronized odometry samples from a single module.

Nested classes/interfaces inherited from class java.lang.Thread

Thread.State, Thread.UncaughtExceptionHandler

Field Summary

Fields

Modifier and Type	Field	Description
static final int	MAX_SAMPLES	Maximum number of odometry samples buffered between drains (250Hz / 50Hz = 5 typical).

Fields inherited from class java.lang.Thread

MAX_PRIORITY, MIN_PRIORITY, NORM_PRIORITY

Constructor Summary

Constructors

Constructor	Description
PhoenixOdometryThread()

Method Summary

Modifier and Type	Method	Description
PhoenixOdometryThread.GyroYawData	getGyroYawData()	Drains all buffered high-frequency gyro yaw samples since the last call.
static PhoenixOdometryThread	getInstance()	Returns the singleton instance, creating and starting the thread on first access.
PhoenixOdometryThread.SyncData	getSyncData(int moduleId)	Drains all buffered odometry samples for a specific module since the last call.
void	registerGyro(com.ctre.phoenix6.BaseStatusSignal yawPos, double odometryHz)	Registers the gyro yaw signal for high-frequency sampling alongside module signals.
int	registerModule(com.ctre.phoenix6.BaseStatusSignal drivePosition, com.ctre.phoenix6.BaseStatusSignal turnPosition, double odometryHz)	Registers a swerve module's drive and turn position signals for high-frequency sampling.
static void	resetInstance()	Stops the current instance and clears the singleton reference.
void	run()

Methods inherited from class java.lang.Thread

activeCount, checkAccess, clone, countStackFrames, currentThread, dumpStack, enumerate, getAllStackTraces, getContextClassLoader, getDefaultUncaughtExceptionHandler, getId, getName, getPriority, getStackTrace, getState, getThreadGroup, getUncaughtExceptionHandler, holdsLock, interrupt, interrupted, isAlive, isDaemon, isInterrupted, join, join, join, onSpinWait, resume, setContextClassLoader, setDaemon, setDefaultUncaughtExceptionHandler, setName, setPriority, setUncaughtExceptionHandler, sleep, sleep, start, stop, suspend, toString, yield

Methods inherited from class java.lang.Object

equals, finalize, getClass, hashCode, notify, notifyAll, wait, wait, wait

Field Details

MAX_SAMPLES

public static final int MAX_SAMPLES

Maximum number of odometry samples buffered between drains (250Hz / 50Hz = 5 typical).

See Also:

• Constant Field Values

Constructor Details

PhoenixOdometryThread

public PhoenixOdometryThread()

Method Details

getInstance

public static PhoenixOdometryThread getInstance()

Returns the singleton instance, creating and starting the thread on first access.

Returns:

The global PhoenixOdometryThread instance.

resetInstance

public static void resetInstance()

Stops the current instance and clears the singleton reference. Only used for unit testing.

registerModule

public int registerModule(com.ctre.phoenix6.BaseStatusSignal drivePosition,
 com.ctre.phoenix6.BaseStatusSignal turnPosition,
 double odometryHz)

Registers a swerve module's drive and turn position signals for high-frequency sampling.

Parameters:

drivePosition - The drive motor's position BaseStatusSignal.

turnPosition - The turn motor's position BaseStatusSignal.

odometryHz - The required update frequency for these signals.

Returns:

The module ID used to retrieve synchronized data via getSyncData(int).

getSyncData

public PhoenixOdometryThread.SyncData getSyncData(int moduleId)

Drains all buffered odometry samples for a specific module since the last call.

Zero-allocation: Returns a pre-allocated PhoenixOdometryThread.SyncData with fixed-capacity arrays. Check PhoenixOdometryThread.SyncData.validCount for the number of populated entries. The returned object is reused — do not store references across ticks.

Parameters:

moduleId - The module ID returned by registerModule(com.ctre.phoenix6.BaseStatusSignal, com.ctre.phoenix6.BaseStatusSignal, double).

Returns:

A pre-allocated PhoenixOdometryThread.SyncData containing synchronized drive/turn positions and timestamps.

registerGyro

public void registerGyro(com.ctre.phoenix6.BaseStatusSignal yawPos,
 double odometryHz)

Registers the gyro yaw signal for high-frequency sampling alongside module signals.

Parameters:

yawPos - The Pigeon2 yaw position BaseStatusSignal.

odometryHz - The required update frequency.

getGyroYawData

public PhoenixOdometryThread.GyroYawData getGyroYawData()

Drains all buffered high-frequency gyro yaw samples since the last call.

Zero-allocation: Returns a pre-allocated PhoenixOdometryThread.GyroYawData with a fixed-capacity array. Check PhoenixOdometryThread.GyroYawData.validCount for the number of populated entries. The returned object is reused — do not store references across ticks.

Returns:

Pre-allocated container with yaw positions (rotations) accumulated since the last drain.

run

public void run()

Specified by:

run in interface Runnable

Overrides:

run in class Thread