Skip to content

Reduce GC churn on phoenix odometry thread #263

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
schveiguy wants to merge 1 commit into
base: main
Choose a base branch
from
+8 −11
Open

Reduce GC churn on phoenix odometry thread #263

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
19 changes: 8 additions & 11 deletions ...ources/talonfx_swerve/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@
public class PhoenixOdometryThread extends Thread {
private final Lock signalsLock =
new ReentrantLock(); // Prevents conflicts when registering signals
private BaseStatusSignal[] phoenixSignals = new BaseStatusSignal[0];
private ArrayList<BaseStatusSignal> phoenixSignals = new ArrayList<>();
private final List<DoubleSupplier> genericSignals = new ArrayList<>();
private final List<Queue<Double>> phoenixQueues = new ArrayList<>();
private final List<Queue<Double>> genericQueues = new ArrayList<>();
Expand Down Expand Up @@ -65,10 +65,7 @@ public Queue<Double> registerSignal(StatusSignal<Angle> signal) {
signalsLock.lock();
Drive.odometryLock.lock();
try {
BaseStatusSignal[] newSignals = new BaseStatusSignal[phoenixSignals.length + 1];
System.arraycopy(phoenixSignals, 0, newSignals, 0, phoenixSignals.length);
newSignals[phoenixSignals.length] = signal;
phoenixSignals = newSignals;
phoenixSignals.add(signal);
phoenixQueues.add(queue);
} finally {
signalsLock.unlock();
Expand Down Expand Up @@ -110,14 +107,14 @@ public void run() {
// Wait for updates from all signals
signalsLock.lock();
try {
if (isCANFD && phoenixSignals.length > 0) {
if (isCANFD && phoenixSignals.size() > 0) {
BaseStatusSignal.waitForAll(2.0 / Drive.ODOMETRY_FREQUENCY, phoenixSignals);
} else {
// "waitForAll" does not support blocking on multiple signals with a bus
// that is not CAN FD, regardless of Pro licensing. No reasoning for this
// behavior is provided by the documentation.
Thread.sleep((long) (1000.0 / Drive.ODOMETRY_FREQUENCY));
if (phoenixSignals.length > 0) BaseStatusSignal.refreshAll(phoenixSignals);
if (phoenixSignals.size() > 0) BaseStatusSignal.refreshAll(phoenixSignals);
}
} catch (InterruptedException e) {
e.printStackTrace();
Expand All @@ -136,13 +133,13 @@ public void run() {
for (BaseStatusSignal signal : phoenixSignals) {
totalLatency += signal.getTimestamp().getLatency();
}
if (phoenixSignals.length > 0) {
timestamp -= totalLatency / phoenixSignals.length;
if (phoenixSignals.size() > 0) {
timestamp -= totalLatency / phoenixSignals.size();
}

// Add new samples to queues
for (int i = 0; i < phoenixSignals.length; i++) {
phoenixQueues.get(i).offer(phoenixSignals[i].getValueAsDouble());
for (int i = 0; i < phoenixSignals.size(); i++) {
phoenixQueues.get(i).offer(phoenixSignals.get(i).getValueAsDouble());
}
for (int i = 0; i < genericSignals.size(); i++) {
genericQueues.get(i).offer(genericSignals.get(i).getAsDouble());
Expand Down