[hal,wpilib] Add function to control "Radio" LED (#6073)

wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotController.java

@@ -6,6 +6,7 @@ package edu.wpi.first.wpilibj;
 
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.hal.HALUtil;
+import edu.wpi.first.hal.LEDJNI;
 import edu.wpi.first.hal.PowerJNI;
 import edu.wpi.first.hal.can.CANJNI;
 import edu.wpi.first.hal.can.CANStatus;
@@ -315,6 +316,66 @@ public final class RobotController {
     return PowerJNI.getCPUTemp();
   }
 
+  /** State for the radio led. */
+  public enum RadioLEDState {
+    /** Off. */
+    kOff(LEDJNI.RADIO_LED_STATE_OFF),
+    /** Green. */
+    kGreen(LEDJNI.RADIO_LED_STATE_GREEN),
+    /** Red. */
+    kRed(LEDJNI.RADIO_LED_STATE_RED),
+    /** Orange. */
+    kOrange(LEDJNI.RADIO_LED_STATE_ORANGE);
+
+    /** The native value for this state. */
+    public final int value;
+
+    RadioLEDState(int value) {
+      this.value = value;
+    }
+
+    /**
+     * Gets a state from an int value.
+     *
+     * @param value int value
+     * @return state
+     */
+    public static RadioLEDState fromValue(int value) {
+      switch (value) {
+        case LEDJNI.RADIO_LED_STATE_OFF:
+          return RadioLEDState.kOff;
+        case LEDJNI.RADIO_LED_STATE_GREEN:
+          return RadioLEDState.kGreen;
+        case LEDJNI.RADIO_LED_STATE_RED:
+          return RadioLEDState.kRed;
+        case LEDJNI.RADIO_LED_STATE_ORANGE:
+          return RadioLEDState.kOrange;
+        default:
+          return RadioLEDState.kOff;
+      }
+    }
+  }
+
+  /**
+   * Set the state of the "Radio" LED. On the RoboRIO, this writes to sysfs, so this function should
+   * not be called multiple times per loop cycle to avoid overruns.
+   *
+   * @param state The state to set the LED to.
+   */
+  public static void setRadioLEDState(RadioLEDState state) {
+    LEDJNI.setRadioLEDState(state.value);
+  }
+
+  /**
+   * Get the state of the "Radio" LED. On the RoboRIO, this reads from sysfs, so this function
+   * should not be called multiple times per loop cycle to avoid overruns.
+   *
+   * @return The state of the LED.
+   */
+  public static RadioLEDState getRadioLEDState() {
+    return RadioLEDState.fromValue(LEDJNI.getRadioLEDState());
+  }
+
   /**
    * Get the current status of the CAN bus.
    *

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/RoboRioSim.java

@@ -6,6 +6,8 @@ package edu.wpi.first.wpilibj.simulation;
 
 import edu.wpi.first.hal.simulation.NotifyCallback;
 import edu.wpi.first.hal.simulation.RoboRioDataJNI;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.RobotController.RadioLEDState;
 
 /** A utility class to control a simulated RoboRIO. */
 public final class RoboRioSim {
@@ -625,6 +627,38 @@ public final class RoboRioSim {
     RoboRioDataJNI.setComments(comments);
   }
 
+  /**
+   * Register a callback to be run whenever the Radio led state changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public static CallbackStore registerRadioLEDStateCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = RoboRioDataJNI.registerRadioLEDStateCallback(callback, initialNotify);
+    return new CallbackStore(uid, RoboRioDataJNI::cancelRadioLEDStateCallback);
+  }
+
+  /**
+   * Get the state of the radio led.
+   *
+   * @return The state of the radio led.
+   */
+  public static RadioLEDState getRadioLEDState() {
+    return RadioLEDState.fromValue(RoboRioDataJNI.getRadioLEDState());
+  }
+
+  /**
+   * Set the state of the radio led.
+   *
+   * @param state The state of the radio led.
+   */
+  public static void setRadioLEDState(RobotController.RadioLEDState state) {
+    RoboRioDataJNI.setRadioLEDState(state.value);
+  }
+
   /** Reset all simulation data. */
   public static void resetData() {
     RoboRioDataJNI.resetData();