Status leds round2 (#1076)

Continuation of #802

Support RGB status LED to indicate:

Running/no-running
NT connected
At least one target visible

Configured by manually uploading hardware config JSON

photon-core/src/main/java/org/photonvision/common/ProgramStatus.java

@@ -20,6 +20,5 @@ package org.photonvision.common;
 public enum ProgramStatus {
     UHOH,
     RUNNING,
-    RUNNING_NT,
-    RUNNING_NT_TARGET
+    RUNNING_NT
 }

photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java

@@ -32,6 +32,7 @@ import org.photonvision.common.configuration.ConfigManager;
 import org.photonvision.common.configuration.NetworkConfig;
 import org.photonvision.common.dataflow.DataChangeService;
 import org.photonvision.common.dataflow.events.OutgoingUIEvent;
+import org.photonvision.common.hardware.HardwareManager;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
 import org.photonvision.common.scripting.ScriptEventType;
@@ -91,6 +92,7 @@ public class NetworkTablesManager {
                                 event.connInfo.remote_port,
                                 event.connInfo.protocol_version);
                 logger.error(msg);
+                HardwareManager.getInstance().setNTConnected(false);
 
                 hasReportedConnectionFailure = true;
                 getInstance().broadcastConnectedStatus();
@@ -102,6 +104,7 @@ public class NetworkTablesManager {
                                 event.connInfo.remote_port,
                                 event.connInfo.protocol_version);
                 logger.info(msg);
+                HardwareManager.getInstance().setNTConnected(true);
 
                 hasReportedConnectionFailure = false;
                 ScriptManager.queueEvent(ScriptEventType.kNTConnected);

photon-core/src/main/java/org/photonvision/common/dataflow/statusLEDs/StatusLEDConsumer.java

@@ -0,0 +1,35 @@
+/*
+ * 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.dataflow.statusLEDs;
+
+import org.photonvision.common.dataflow.CVPipelineResultConsumer;
+import org.photonvision.common.hardware.HardwareManager;
+import org.photonvision.vision.pipeline.result.CVPipelineResult;
+
+public class StatusLEDConsumer implements CVPipelineResultConsumer {
+    private final int index;
+
+    public StatusLEDConsumer(int index) {
+        this.index = index;
+    }
+
+    @Override
+    public void accept(CVPipelineResult t) {
+        HardwareManager.getInstance().setTargetsVisibleStatus(this.index, t.hasTargets());
+    }
+}

photon-core/src/main/java/org/photonvision/common/hardware/HardwareManager.java

@@ -20,7 +20,8 @@ package org.photonvision.common.hardware;
 import edu.wpi.first.networktables.IntegerPublisher;
 import edu.wpi.first.networktables.IntegerSubscriber;
 import java.io.IOException;
-import org.photonvision.common.ProgramStatus;
+import java.util.HashMap;
+import java.util.Map;
 import org.photonvision.common.configuration.ConfigManager;
 import org.photonvision.common.configuration.HardwareConfig;
 import org.photonvision.common.configuration.HardwareSettings;
@@ -32,6 +33,7 @@ import org.photonvision.common.hardware.metrics.MetricsManager;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
 import org.photonvision.common.util.ShellExec;
+import org.photonvision.common.util.TimedTaskManager;
 
 public class HardwareManager {
     private static HardwareManager instance;
@@ -96,6 +98,10 @@ public class HardwareManager {
                         ? new StatusLED(hardwareConfig.statusRGBPins)
                         : null;
 
+        if (statusLED != null) {
+            TimedTaskManager.getInstance().addTask("StatusLEDUpdate", this::statusLEDUpdate, 150);
+        }
+
         var hasBrightnessRange = hardwareConfig.ledBrightnessRange.size() == 2;
         visionLED =
                 hardwareConfig.ledPins.isEmpty()
@@ -160,21 +166,61 @@ public class HardwareManager {
         }
     }
 
-    public void setStatus(ProgramStatus status) {
-        switch (status) {
-            case UHOH:
-                // red flashing, green off
-                break;
-            case RUNNING:
-                // red solid, green off
-                break;
-            case RUNNING_NT:
-                // red off, green solid
-                break;
-            case RUNNING_NT_TARGET:
-                // red off, green flashing
-                break;
+    // API's supporting status LEDs
+
+    private Map<Integer, Boolean> pipelineTargets = new HashMap<Integer, Boolean>();
+    private boolean ntConnected = false;
+    private boolean systemRunning = false;
+    private int blinkCounter = 0;
+
+    public void setTargetsVisibleStatus(int pipelineIdx, boolean hasTargets) {
+        pipelineTargets.put(pipelineIdx, hasTargets);
+    }
+
+    public void setNTConnected(boolean isConnected) {
+        this.ntConnected = isConnected;
+    }
+
+    public void setRunning(boolean isRunning) {
+        this.systemRunning = isRunning;
+    }
+
+    private void statusLEDUpdate() {
+        // make blinky
+        boolean blinky = ((blinkCounter % 3) > 0);
+
+        // check if any pipeline has a visible target
+        boolean anyTarget = false;
+        for (var t : this.pipelineTargets.values()) {
+            if (t) {
+                anyTarget = true;
+            }
         }
+
+        if (this.systemRunning) {
+            if (!this.ntConnected) {
+                if (anyTarget) {
+                    // Blue Flashing
+                    statusLED.setRGB(false, false, blinky);
+                } else {
+                    // Yellow flashing
+                    statusLED.setRGB(blinky, blinky, false);
+                }
+            } else {
+                if (anyTarget) {
+                    // Blue
+                    statusLED.setRGB(false, false, blinky);
+                } else {
+                    // blinky green
+                    statusLED.setRGB(false, blinky, false);
+                }
+            }
+        } else {
+            // Faulted, not running... blinky red
+            statusLED.setRGB(blinky, false, false);
+        }
+
+        blinkCounter++;
     }
 
     public HardwareConfig getConfig() {

photon-core/src/main/java/org/photonvision/common/hardware/StatusLED.java

@@ -45,4 +45,14 @@ public class StatusLED {
             blueLED = new CustomGPIO(statusLedPins.get(2));
         }
     }
+
+    public void setRGB(boolean r, boolean g, boolean b) {
+        // Outputs are active-low, so invert the level applied
+        redLED.setState(!r);
+        redLED.setBrightness(r ? 0 : 100);
+        greenLED.setState(!g);
+        greenLED.setBrightness(g ? 0 : 100);
+        blueLED.setState(!b);
+        blueLED.setBrightness(b ? 0 : 100);
+    }
 }

photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java

@@ -33,6 +33,7 @@ import org.photonvision.common.dataflow.CVPipelineResultConsumer;
 import org.photonvision.common.dataflow.DataChangeService;
 import org.photonvision.common.dataflow.events.OutgoingUIEvent;
 import org.photonvision.common.dataflow.networktables.NTDataPublisher;
+import org.photonvision.common.dataflow.statusLEDs.StatusLEDConsumer;
 import org.photonvision.common.dataflow.websocket.UIDataPublisher;
 import org.photonvision.common.hardware.HardwareManager;
 import org.photonvision.common.logging.LogGroup;
@@ -72,6 +73,7 @@ public class VisionModule {
             new LinkedList<>();
     private final NTDataPublisher ntConsumer;
     private final UIDataPublisher uiDataConsumer;
+    private final StatusLEDConsumer statusLEDsConsumer;
     protected final int moduleIndex;
     protected final QuirkyCamera cameraQuirks;
 
@@ -143,8 +145,10 @@ public class VisionModule {
                         pipelineManager::getDriverMode,
                         this::setDriverMode);
         uiDataConsumer = new UIDataPublisher(index);
+        statusLEDsConsumer = new StatusLEDConsumer(index);
         addResultConsumer(ntConsumer);
         addResultConsumer(uiDataConsumer);
+        addResultConsumer(statusLEDsConsumer);
         addResultConsumer(
                 (result) ->
                         lastPipelineResultBestTarget = result.hasTargets() ? result.targets.get(0) : null);

photon-server/src/main/java/org/photonvision/Main.java

@@ -375,6 +375,7 @@ public class Main {
         }
 
         logger.info("Starting server...");
+        HardwareManager.getInstance().setRunning(true);
         Server.start(DEFAULT_WEBPORT);
     }
 }