001/*
002 * Copyright (C) Photon Vision.
003 *
004 * This program is free software: you can redistribute it and/or modify
005 * it under the terms of the GNU General Public License as published by
006 * the Free Software Foundation, either version 3 of the License, or
007 * (at your option) any later version.
008 *
009 * This program is distributed in the hope that it will be useful,
010 * but WITHOUT ANY WARRANTY; without even the implied warranty of
011 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
012 * GNU General Public License for more details.
013 *
014 * You should have received a copy of the GNU General Public License
015 * along with this program.  If not, see <https://www.gnu.org/licenses/>.
016 */
017
018package org.photonvision.common.hardware;
019
020import edu.wpi.first.networktables.IntegerPublisher;
021import edu.wpi.first.networktables.IntegerSubscriber;
022import java.io.IOException;
023import java.util.HashMap;
024import java.util.Map;
025import org.photonvision.common.configuration.ConfigManager;
026import org.photonvision.common.configuration.HardwareConfig;
027import org.photonvision.common.configuration.HardwareSettings;
028import org.photonvision.common.dataflow.networktables.NTDataChangeListener;
029import org.photonvision.common.dataflow.networktables.NetworkTablesManager;
030import org.photonvision.common.hardware.GPIO.CustomGPIO;
031import org.photonvision.common.hardware.GPIO.pi.PigpioSocket;
032import org.photonvision.common.hardware.metrics.MetricsManager;
033import org.photonvision.common.logging.LogGroup;
034import org.photonvision.common.logging.Logger;
035import org.photonvision.common.util.ShellExec;
036import org.photonvision.common.util.TimedTaskManager;
037
038public class HardwareManager {
039    private static HardwareManager instance;
040
041    private final ShellExec shellExec = new ShellExec(true, false);
042    private final Logger logger = new Logger(HardwareManager.class, LogGroup.General);
043
044    private final HardwareConfig hardwareConfig;
045    private final HardwareSettings hardwareSettings;
046
047    private final MetricsManager metricsManager;
048
049    @SuppressWarnings({"FieldCanBeLocal", "unused"})
050    private final StatusLED statusLED;
051
052    @SuppressWarnings("FieldCanBeLocal")
053    private final IntegerSubscriber ledModeRequest;
054
055    private final IntegerPublisher ledModeState;
056
057    @SuppressWarnings({"FieldCanBeLocal", "unused"})
058    private final NTDataChangeListener ledModeListener;
059
060    public final VisionLED visionLED; // May be null if no LED is specified
061
062    private final PigpioSocket pigpioSocket; // will be null unless on Raspi
063
064    public static HardwareManager getInstance() {
065        if (instance == null) {
066            var conf = ConfigManager.getInstance().getConfig();
067            instance = new HardwareManager(conf.getHardwareConfig(), conf.getHardwareSettings());
068        }
069        return instance;
070    }
071
072    private HardwareManager(HardwareConfig hardwareConfig, HardwareSettings hardwareSettings) {
073        this.hardwareConfig = hardwareConfig;
074        this.hardwareSettings = hardwareSettings;
075
076        this.metricsManager = new MetricsManager();
077        this.metricsManager.setConfig(hardwareConfig);
078
079        ledModeRequest =
080                NetworkTablesManager.getInstance()
081                        .kRootTable
082                        .getIntegerTopic("ledModeRequest")
083                        .subscribe(-1);
084        ledModeState =
085                NetworkTablesManager.getInstance().kRootTable.getIntegerTopic("ledModeState").publish();
086        ledModeState.set(VisionLEDMode.kDefault.value);
087
088        CustomGPIO.setConfig(hardwareConfig);
089
090        if (Platform.isRaspberryPi()) {
091            pigpioSocket = new PigpioSocket();
092        } else {
093            pigpioSocket = null;
094        }
095
096        statusLED =
097                hardwareConfig.statusRGBPins.size() == 3
098                        ? new StatusLED(hardwareConfig.statusRGBPins)
099                        : null;
100
101        if (statusLED != null) {
102            TimedTaskManager.getInstance().addTask("StatusLEDUpdate", this::statusLEDUpdate, 150);
103        }
104
105        var hasBrightnessRange = hardwareConfig.ledBrightnessRange.size() == 2;
106        visionLED =
107                hardwareConfig.ledPins.isEmpty()
108                        ? null
109                        : new VisionLED(
110                                hardwareConfig.ledPins,
111                                hasBrightnessRange ? hardwareConfig.ledBrightnessRange.get(0) : 0,
112                                hasBrightnessRange ? hardwareConfig.ledBrightnessRange.get(1) : 100,
113                                pigpioSocket,
114                                ledModeState::set);
115
116        ledModeListener =
117                visionLED == null
118                        ? null
119                        : new NTDataChangeListener(
120                                NetworkTablesManager.getInstance().kRootTable.getInstance(),
121                                ledModeRequest,
122                                visionLED::onLedModeChange);
123
124        Runtime.getRuntime().addShutdownHook(new Thread(this::onJvmExit));
125
126        if (visionLED != null) {
127            visionLED.setBrightness(hardwareSettings.ledBrightnessPercentage);
128            visionLED.blink(85, 4); // bootup blink
129        }
130
131        // Start hardware metrics thread (Disabled until implemented)
132        // if (Platform.isLinux()) MetricsPublisher.getInstance().startTask();
133    }
134
135    public void setBrightnessPercent(int percent) {
136        if (percent != hardwareSettings.ledBrightnessPercentage) {
137            hardwareSettings.ledBrightnessPercentage = percent;
138            if (visionLED != null) visionLED.setBrightness(percent);
139            ConfigManager.getInstance().requestSave();
140            logger.info("Setting led brightness to " + percent + "%");
141        }
142    }
143
144    private void onJvmExit() {
145        logger.info("Shutting down LEDs...");
146        if (visionLED != null) visionLED.setState(false);
147
148        ConfigManager.getInstance().onJvmExit();
149    }
150
151    public boolean restartDevice() {
152        if (Platform.isLinux()) {
153            try {
154                return shellExec.executeBashCommand("reboot now") == 0;
155            } catch (IOException e) {
156                logger.error("Could not restart device!", e);
157                return false;
158            }
159        }
160        try {
161            return shellExec.executeBashCommand(hardwareConfig.restartHardwareCommand) == 0;
162        } catch (IOException e) {
163            logger.error("Could not restart device!", e);
164            return false;
165        }
166    }
167
168    // API's supporting status LEDs
169
170    private Map<String, Boolean> pipelineTargets = new HashMap<String, Boolean>();
171    private boolean ntConnected = false;
172    private boolean systemRunning = false;
173    private int blinkCounter = 0;
174
175    public void setTargetsVisibleStatus(String uniqueName, boolean hasTargets) {
176        pipelineTargets.put(uniqueName, hasTargets);
177    }
178
179    public void setNTConnected(boolean isConnected) {
180        this.ntConnected = isConnected;
181    }
182
183    public void setRunning(boolean isRunning) {
184        this.systemRunning = isRunning;
185    }
186
187    private void statusLEDUpdate() {
188        // make blinky
189        boolean blinky = ((blinkCounter % 3) > 0);
190
191        // check if any pipeline has a visible target
192        boolean anyTarget = false;
193        for (var t : this.pipelineTargets.values()) {
194            if (t) {
195                anyTarget = true;
196            }
197        }
198
199        if (this.systemRunning) {
200            if (!this.ntConnected) {
201                if (anyTarget) {
202                    // Blue Flashing
203                    statusLED.setRGB(false, false, blinky);
204                } else {
205                    // Yellow flashing
206                    statusLED.setRGB(blinky, blinky, false);
207                }
208            } else {
209                if (anyTarget) {
210                    // Blue
211                    statusLED.setRGB(false, false, blinky);
212                } else {
213                    // blinky green
214                    statusLED.setRGB(false, blinky, false);
215                }
216            }
217        } else {
218            // Faulted, not running... blinky red
219            statusLED.setRGB(blinky, false, false);
220        }
221
222        blinkCounter++;
223    }
224
225    public HardwareConfig getConfig() {
226        return hardwareConfig;
227    }
228
229    public void publishMetrics() {
230        metricsManager.publishMetrics();
231    }
232}