Add support for Green/Yellow status LEDs, like is used on some Limelights (#2287)

2026-06-29 02:21:41 +00:00 · 2026-05-20 12:46:41 -04:00
parent 7e9a67ec6a
commit 02d0f2b047
16 changed files with 387 additions and 72 deletions
--- a/photon-core/src/main/java/org/photonvision/common/configuration/HardwareConfig.java
+++ b/photon-core/src/main/java/org/photonvision/common/configuration/HardwareConfig.java
@@ -17,8 +17,10 @@

 package org.photonvision.common.configuration;

+import com.fasterxml.jackson.annotation.JsonAlias;
 import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
 import java.util.ArrayList;
+import org.photonvision.common.hardware.statusLED.StatusLEDType;

@JsonIgnoreProperties(ignoreUnknown = true)
 public class HardwareConfig {
@@ -29,8 +31,13 @@ public class HardwareConfig {
    public final boolean ledsCanDim;
    public final ArrayList<Integer> ledBrightnessRange;
    public final int ledPWMFrequency;
-    public final ArrayList<Integer> statusRGBPins;
-    public final boolean statusRGBActiveHigh;
+    public final StatusLEDType statusLEDType;
+
+    @JsonAlias("statusRGBPins")
+    public final ArrayList<Integer> statusLEDPins;
+
+    @JsonAlias("statusRGBActiveHigh")
+    public final boolean statusLEDActiveHigh;

    // Custom GPIO
    public final String getGPIOCommand;
@@ -49,8 +56,9 @@ public class HardwareConfig {
            boolean ledsCanDim,
            ArrayList<Integer> ledBrightnessRange,
            int ledPwmFrequency,
-            ArrayList<Integer> statusRGBPins,
-            boolean statusRGBActiveHigh,
+            StatusLEDType statusLEDType,
+            ArrayList<Integer> statusLEDPins,
+            boolean statusLEDActiveHigh,
            String getGPIOCommand,
            String setGPIOCommand,
            String setPWMCommand,
@@ -63,8 +71,9 @@ public class HardwareConfig {
        this.ledsCanDim = ledsCanDim;
        this.ledBrightnessRange = ledBrightnessRange;
        this.ledPWMFrequency = ledPwmFrequency;
-        this.statusRGBPins = statusRGBPins;
-        this.statusRGBActiveHigh = statusRGBActiveHigh;
+        this.statusLEDType = statusLEDType;
+        this.statusLEDPins = statusLEDPins;
+        this.statusLEDActiveHigh = statusLEDActiveHigh;
        this.getGPIOCommand = getGPIOCommand;
        this.setGPIOCommand = setGPIOCommand;
        this.setPWMCommand = setPWMCommand;
@@ -80,8 +89,9 @@ public class HardwareConfig {
        ledsCanDim = false;
        ledBrightnessRange = new ArrayList<>();
        ledPWMFrequency = 0;
-        statusRGBPins = new ArrayList<>();
-        statusRGBActiveHigh = false;
+        statusLEDType = StatusLEDType.RGB;
+        statusLEDPins = new ArrayList<>();
+        statusLEDActiveHigh = false;
        getGPIOCommand = "";
        setGPIOCommand = "";
        setPWMCommand = "";
@@ -121,10 +131,12 @@ public class HardwareConfig {
                + ledBrightnessRange
                + ", ledPWMFrequency="
                + ledPWMFrequency
-                + ", statusRGBPins="
-                + statusRGBPins
-                + ", statusRGBActiveHigh"
-                + statusRGBActiveHigh
+                + ", statusLEDType="
+                + statusLEDType
+                + ", statusLEDPins="
+                + statusLEDPins
+                + ", statusLEDActiveHigh"
+                + statusLEDActiveHigh
                + ", getGPIOCommand="
                + getGPIOCommand
                + ", setGPIOCommand="
--- a/photon-core/src/main/java/org/photonvision/common/hardware/HardwareManager.java
+++ b/photon-core/src/main/java/org/photonvision/common/hardware/HardwareManager.java
@@ -24,6 +24,7 @@ import com.diozero.sbc.DeviceFactoryHelper;
 import java.io.IOException;
 import java.util.HashSet;
 import java.util.List;
+import java.util.Optional;
 import java.util.Set;
 import java.util.function.Supplier;
 import org.photonvision.common.configuration.ConfigManager;
@@ -33,6 +34,7 @@ import org.photonvision.common.dataflow.networktables.NTDataChangeListener;
 import org.photonvision.common.dataflow.networktables.NetworkTablesManager;
 import org.photonvision.common.hardware.gpio.CustomAdapter;
 import org.photonvision.common.hardware.gpio.CustomDeviceFactory;
+import org.photonvision.common.hardware.statusLED.StatusLED;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
 import org.photonvision.common.util.ShellExec;
@@ -48,18 +50,15 @@ public class HardwareManager {
    private final HardwareConfig hardwareConfig;
    private final HardwareSettings hardwareSettings;

-    @SuppressWarnings({"FieldCanBeLocal", "unused"})
-    private final StatusLED statusLED;
+    private final Optional<StatusLED> statusLED;

-    @SuppressWarnings("FieldCanBeLocal")
    private final IntegerSubscriber ledModeRequest;

    private final IntegerPublisher ledModeState;

-    @SuppressWarnings({"FieldCanBeLocal", "unused"})
-    private final NTDataChangeListener ledModeListener;
+    private final Optional<NTDataChangeListener> ledModeListener;

-    public final VisionLED visionLED; // May be null if no LED is specified
+    public final Optional<VisionLED> visionLED;

    public static HardwareManager getInstance() {
        if (instance == null) {
@@ -102,40 +101,44 @@ public class HardwareManager {
                };

        statusLED =
-                hardwareConfig.statusRGBPins.size() == 3
-                        ? new StatusLED(
-                                lazyDeviceFactory.get(),
-                                hardwareConfig.statusRGBPins,
-                                hardwareConfig.statusRGBActiveHigh)
-                        : null;
+                hardwareConfig.statusLEDPins.isEmpty()
+                        ? Optional.empty()
+                        : Optional.of(
+                                StatusLED.ofType(
+                                        hardwareConfig.statusLEDType,
+                                        lazyDeviceFactory,
+                                        hardwareConfig.statusLEDPins,
+                                        hardwareConfig.statusLEDActiveHigh));

        var hasBrightnessRange = hardwareConfig.ledBrightnessRange.size() == 2;
        visionLED =
                hardwareConfig.ledPins.isEmpty()
-                        ? null
-                        : new VisionLED(
-                                lazyDeviceFactory.get(),
-                                hardwareConfig.ledPins,
-                                hardwareConfig.ledsCanDim,
-                                hasBrightnessRange ? hardwareConfig.ledBrightnessRange.get(0) : 0,
-                                hasBrightnessRange ? hardwareConfig.ledBrightnessRange.get(1) : 100,
-                                hardwareConfig.ledPWMFrequency,
-                                ledModeState::set);
+                        ? Optional.empty()
+                        : Optional.of(
+                                new VisionLED(
+                                        lazyDeviceFactory.get(),
+                                        hardwareConfig.ledPins,
+                                        hardwareConfig.ledsCanDim,
+                                        hasBrightnessRange ? hardwareConfig.ledBrightnessRange.get(0) : 0,
+                                        hasBrightnessRange ? hardwareConfig.ledBrightnessRange.get(1) : 100,
+                                        hardwareConfig.ledPWMFrequency,
+                                        ledModeState::set));

        ledModeListener =
-                visionLED == null
-                        ? null
-                        : new NTDataChangeListener(
-                                NetworkTablesManager.getInstance().kRootTable.getInstance(),
-                                ledModeRequest,
-                                visionLED::onLedModeChange);
+                visionLED.map(
+                        visionLED ->
+                                new NTDataChangeListener(
+                                        NetworkTablesManager.getInstance().kRootTable.getInstance(),
+                                        ledModeRequest,
+                                        visionLED::onLedModeChange));

        Runtime.getRuntime().addShutdownHook(new Thread(this::onJvmExit));

-        if (visionLED != null) {
-            visionLED.setBrightness(hardwareSettings.ledBrightnessPercentage);
-            visionLED.blink(85, 4); // bootup blink
-        }
+        visionLED.ifPresent(
+                visionLED -> {
+                    visionLED.setBrightness(hardwareSettings.ledBrightnessPercentage);
+                    visionLED.blink(85, 4); // bootup blink
+                });

        // Start hardware metrics thread (Disabled until implemented)
        // if (Platform.isLinux()) MetricsPublisher.getInstance().startTask();
@@ -161,7 +164,7 @@ public class HardwareManager {
                pinInfo.addGpioPinInfo(pin, pin, List.of(DeviceMode.DIGITAL_OUTPUT));
            }
        }
-        for (int pin : hardwareConfig.statusRGBPins) {
+        for (int pin : hardwareConfig.statusLEDPins) {
            pinInfo.addGpioPinInfo(pin, pin, List.of(DeviceMode.DIGITAL_OUTPUT));
        }

@@ -171,7 +174,7 @@ public class HardwareManager {
    public void setBrightnessPercent(int percent) {
        if (percent != hardwareSettings.ledBrightnessPercentage) {
            hardwareSettings.ledBrightnessPercentage = percent;
-            if (visionLED != null) visionLED.setBrightness(percent);
+            visionLED.ifPresent(visionLED -> visionLED.setBrightness(percent));
            ConfigManager.getInstance().requestSave();
            logger.info("Setting led brightness to " + percent + "%");
        }
@@ -179,7 +182,7 @@ public class HardwareManager {

    private void onJvmExit() {
        logger.info("Shutting down LEDs...");
-        if (visionLED != null) visionLED.setState(false);
+        visionLED.ifPresent(visionLED -> visionLED.setState(false));

        ConfigManager.getInstance().onJvmExit();
    }
@@ -220,16 +223,16 @@ public class HardwareManager {
        updateStatus();
    }

-    public void setError(PhotonStatus status) {
-        if (status == null || !status.isError()) {
+    public void setError(Optional<PhotonStatus> status) {
+        if (status.isEmpty() || !status.get().isError()) {
            updateStatus();
-        } else if (statusLED != null) {
-            statusLED.setStatus(status);
+        } else {
+            statusLED.ifPresent(statusLED -> statusLED.setStatus(status.get()));
        }
    }

    private void updateStatus() {
-        if (statusLED == null) {
+        if (statusLED.isEmpty()) {
            return;
        }
        PhotonStatus status;
@@ -247,6 +250,6 @@ public class HardwareManager {
                status = PhotonStatus.NT_DISCONNECTED_TARGETS_MISSING;
            }
        }
-        statusLED.setStatus(status);
+        statusLED.ifPresent(statusLED -> statusLED.setStatus(status));
    }
 }
--- a/photon-core/src/main/java/org/photonvision/common/hardware/statusLED/GreenYellowStatusLED.java
+++ b/photon-core/src/main/java/org/photonvision/common/hardware/statusLED/GreenYellowStatusLED.java
@@ -0,0 +1,99 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.common.hardware.statusLED;
+
+import com.diozero.devices.LED;
+import com.diozero.internal.spi.NativeDeviceFactoryInterface;
+import java.util.Collections;
+import java.util.List;
+import org.photonvision.common.hardware.PhotonStatus;
+import org.photonvision.common.logging.LogGroup;
+import org.photonvision.common.logging.Logger;
+import org.photonvision.common.util.TimedTaskManager;
+
+/** A pair of green and yellow LEDs, as used on the Limelight cameras */
+public class GreenYellowStatusLED implements StatusLED {
+    private final Logger logger = new Logger(GreenYellowStatusLED.class, LogGroup.General);
+
+    public final LED greenLED;
+    public final LED yellowLED;
+    protected int blinkCounter;
+
+    protected PhotonStatus status = PhotonStatus.GENERIC_ERROR;
+
+    public GreenYellowStatusLED(
+            NativeDeviceFactoryInterface deviceFactory, List<Integer> statusLedPins, boolean activeHigh) {
+        if (statusLedPins.size() != 2) {
+            logger.warn(
+                    pinErrorTemplate.formatted(2, "Green and Yellow status LEDs", statusLedPins.size()));
+        }
+        // fill unassigned pins with -1 to disable
+        if (statusLedPins.size() < 2) {
+            statusLedPins.addAll(Collections.nCopies(statusLedPins.size() - 2, -1));
+        }
+
+        // Outputs are active-low for a common-anode RGB LED
+        greenLED = new LED(deviceFactory, statusLedPins.get(0), activeHigh, false);
+        yellowLED = new LED(deviceFactory, statusLedPins.get(1), activeHigh, false);
+
+        TimedTaskManager.getInstance().addTask("StatusLEDUpdate", this::updateLED, 75);
+    }
+
+    protected void setLEDs(boolean green, boolean yellow) {
+        greenLED.setOn(green);
+        yellowLED.setOn(yellow);
+    }
+
+    @Override
+    public void setStatus(PhotonStatus status) {
+        this.status = status;
+    }
+
+    protected void updateLED() {
+        boolean slowBlink = (blinkCounter % 6) > 1;
+        boolean fastBlink = (blinkCounter % 2) > 0;
+        boolean errorBlink = blinkCounter > 5;
+
+        switch (status) {
+            case NT_CONNECTED_TARGETS_VISIBLE ->
+                    // Green fast, yellow on
+                    setLEDs(fastBlink, true);
+            case NT_CONNECTED_TARGETS_MISSING ->
+                    // Green slow, yellow on
+                    setLEDs(slowBlink, true);
+            case NT_DISCONNECTED_TARGETS_VISIBLE ->
+                    // Green fast, yellow slow
+                    setLEDs(fastBlink, slowBlink);
+            case NT_DISCONNECTED_TARGETS_MISSING ->
+                    // Green slow, yellow slow
+                    setLEDs(slowBlink, slowBlink);
+            case GENERIC_ERROR ->
+                    // Extra slow alternating blink
+                    setLEDs(errorBlink, !errorBlink);
+        }
+
+        blinkCounter++;
+        blinkCounter %= 12;
+    }
+
+    @Override
+    public void close() throws Exception {
+        greenLED.close();
+        yellowLED.close();
+    }
+}
--- a/photon-core/src/main/java/org/photonvision/common/hardware/statusLED/RGBStatusLED.java
+++ b/photon-core/src/main/java/org/photonvision/common/hardware/statusLED/RGBStatusLED.java
@@ -15,14 +15,21 @@
 * along with this program.  If not, see <https://www.gnu.org/licenses/>.
 */

-package org.photonvision.common.hardware;
+package org.photonvision.common.hardware.statusLED;

 import com.diozero.devices.LED;
 import com.diozero.internal.spi.NativeDeviceFactoryInterface;
+import java.util.Collections;
 import java.util.List;
+import org.photonvision.common.hardware.PhotonStatus;
+import org.photonvision.common.logging.LogGroup;
+import org.photonvision.common.logging.Logger;
 import org.photonvision.common.util.TimedTaskManager;

-public class StatusLED implements AutoCloseable {
+/** Basic RGB LED with individual control over each pin */
+public class RGBStatusLED implements StatusLED {
+    private final Logger logger = new Logger(RGBStatusLED.class, LogGroup.General);
+
    public final LED redLED;
    public final LED greenLED;
    public final LED blueLED;
@@ -30,13 +37,14 @@ public class StatusLED implements AutoCloseable {

    protected PhotonStatus status = PhotonStatus.GENERIC_ERROR;

-    public StatusLED(
+    public RGBStatusLED(
            NativeDeviceFactoryInterface deviceFactory, List<Integer> statusLedPins, boolean activeHigh) {
-        // fill unassigned pins with -1 to disable
        if (statusLedPins.size() != 3) {
-            for (int i = 0; i < 3 - statusLedPins.size(); i++) {
-                statusLedPins.add(-1);
-            }
+            logger.warn(pinErrorTemplate.formatted(3, "a RGB status LED", statusLedPins.size()));
+        }
+        // fill unassigned pins with -1 to disable
+        if (statusLedPins.size() < 3) {
+            statusLedPins.addAll(Collections.nCopies(statusLedPins.size() - 3, -1));
        }

        // Outputs are active-low for a common-anode RGB LED
@@ -53,6 +61,7 @@ public class StatusLED implements AutoCloseable {
        blueLED.setOn(b);
    }

+    @Override
    public void setStatus(PhotonStatus status) {
        this.status = status;
    }
--- a/photon-core/src/main/java/org/photonvision/common/hardware/statusLED/StatusLED.java
+++ b/photon-core/src/main/java/org/photonvision/common/hardware/statusLED/StatusLED.java
@@ -0,0 +1,42 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.common.hardware.statusLED;
+
+import com.diozero.internal.spi.NativeDeviceFactoryInterface;
+import java.util.List;
+import java.util.function.Supplier;
+import org.photonvision.common.hardware.PhotonStatus;
+
+public interface StatusLED extends AutoCloseable {
+    static final String pinErrorTemplate =
+            "Expected %d pins for %s, but found %n pins; unassigned pins will be skipped, extra pins will be ignored";
+
+    public void setStatus(PhotonStatus status);
+
+    static StatusLED ofType(
+            StatusLEDType type,
+            Supplier<NativeDeviceFactoryInterface> lazyDeviceFactory,
+            List<Integer> statusLedPins,
+            boolean activeHigh) {
+        return switch (type) {
+            case RGB -> new RGBStatusLED(lazyDeviceFactory.get(), statusLedPins, activeHigh);
+            case GreenYellow ->
+                    new GreenYellowStatusLED(lazyDeviceFactory.get(), statusLedPins, activeHigh);
+        };
+    }
+}
--- a/photon-core/src/main/java/org/photonvision/common/hardware/statusLED/StatusLEDType.java
+++ b/photon-core/src/main/java/org/photonvision/common/hardware/statusLED/StatusLEDType.java
@@ -0,0 +1,23 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.common.hardware.statusLED;
+
+public enum StatusLEDType {
+    RGB,
+    GreenYellow;
+}
--- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java
+++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java
@@ -182,7 +182,10 @@ public class VisionModule {
        if (HardwareManager.getInstance().visionLED != null && this.camShouldControlLEDs()) {
            HardwareManager.getInstance()
                    .visionLED
-                    .setPipelineModeSupplier(() -> pipelineManager.getCurrentPipelineSettings().ledMode);
+                    .ifPresent(
+                            (visionLED) ->
+                                    visionLED.setPipelineModeSupplier(
+                                            () -> pipelineManager.getCurrentPipelineSettings().ledMode));
            setVisionLEDs(pipelineManager.getCurrentPipelineSettings().ledMode);
        }

@@ -517,8 +520,9 @@ public class VisionModule {
    }

    private void setVisionLEDs(boolean on) {
-        if (camShouldControlLEDs() && HardwareManager.getInstance().visionLED != null)
-            HardwareManager.getInstance().visionLED.setState(on);
+        if (camShouldControlLEDs()) {
+            HardwareManager.getInstance().visionLED.ifPresent((visionLED) -> visionLED.setState(on));
+        }
    }

    public void saveModule() {