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

wpilibc/src/main/native/cpp/RobotController.cpp

@@ -8,6 +8,7 @@
 
 #include <hal/CAN.h>
 #include <hal/HALBase.h>
+#include <hal/LEDs.h>
 #include <hal/Power.h>
 
 #include "frc/Errors.h"
@@ -230,6 +231,30 @@ units::celsius_t RobotController::GetCPUTemp() {
   return units::celsius_t{retVal};
 }
 
+static_assert(RadioLEDState::kOff ==
+              static_cast<RadioLEDState>(HAL_RadioLEDState::HAL_RadioLED_kOff));
+static_assert(
+    RadioLEDState::kGreen ==
+    static_cast<RadioLEDState>(HAL_RadioLEDState::HAL_RadioLED_kGreen));
+static_assert(RadioLEDState::kRed ==
+              static_cast<RadioLEDState>(HAL_RadioLEDState::HAL_RadioLED_kRed));
+static_assert(
+    RadioLEDState::kOrange ==
+    static_cast<RadioLEDState>(HAL_RadioLEDState::HAL_RadioLED_kOrange));
+
+void RobotController::SetRadioLEDState(RadioLEDState state) {
+  int32_t status = 0;
+  HAL_SetRadioLEDState(static_cast<HAL_RadioLEDState>(state), &status);
+  FRC_CheckErrorStatus(status, "SetRadioLEDState");
+}
+
+RadioLEDState RobotController::GetRadioLEDState() {
+  int32_t status = 0;
+  auto retVal = static_cast<RadioLEDState>(HAL_GetRadioLEDState(&status));
+  FRC_CheckErrorStatus(status, "GetRadioLEDState");
+  return retVal;
+}
+
 CANStatus RobotController::GetCANStatus() {
   int32_t status = 0;
   float percentBusUtilization = 0;

wpilibc/src/main/native/cpp/simulation/RoboRioSim.cpp

@@ -338,6 +338,23 @@ void RoboRioSim::SetComments(std::string_view comments) {
   HALSIM_SetRoboRioComments(comments.data(), comments.size());
 }
 
+std::unique_ptr<CallbackStore> RoboRioSim::RegisterRadioLEDStateCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelRoboRioRadioLEDStateCallback);
+  store->SetUid(HALSIM_RegisterRoboRioRadioLEDStateCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+RadioLEDState RoboRioSim::GetRadioLEDState() {
+  return static_cast<RadioLEDState>(HALSIM_GetRoboRioRadioLEDState());
+}
+
+void RoboRioSim::SetRadioLEDState(RadioLEDState state) {
+  HALSIM_SetRoboRioRadioLEDState(static_cast<HAL_RadioLEDState>(state));
+}
+
 void RoboRioSim::ResetData() {
   HALSIM_ResetRoboRioData();
 }

wpilibc/src/main/native/include/frc/RobotController.h

@@ -21,6 +21,8 @@ struct CANStatus {
   int transmitErrorCount;
 };
 
+enum RadioLEDState { kOff = 0, kGreen = 1, kRed = 2, kOrange = 3 };
+
 class RobotController {
  public:
   RobotController() = delete;
@@ -274,6 +276,23 @@ class RobotController {
    */
   static units::celsius_t GetCPUTemp();
 
+  /**
+   * 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.
+   */
+  static void SetRadioLEDState(RadioLEDState state);
+
+  /**
+   * 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.
+   */
+  static RadioLEDState GetRadioLEDState();
+
   /**
    * Get the current status of the CAN bus.
    *

wpilibc/src/main/native/include/frc/simulation/RoboRioSim.h

@@ -11,6 +11,7 @@
 #include <units/temperature.h>
 #include <units/voltage.h>
 
+#include "frc/RobotController.h"
 #include "frc/simulation/CallbackStore.h"
 
 namespace frc::sim {
@@ -514,6 +515,32 @@ class RoboRioSim {
    */
   static void SetComments(std::string_view comments);
 
+  /**
+   * Register a callback to be run whenever the Radio led state changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the
+   *                      initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  static std::unique_ptr<CallbackStore> RegisterRadioLEDStateCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Get the state of the radio led.
+   *
+   * @return The state of the radio led.
+   */
+  static RadioLEDState GetRadioLEDState();
+
+  /**
+   * Set the state of the radio led.
+   *
+   * @param state The state of the radio led.
+   */
+  static void SetRadioLEDState(RadioLEDState state);
+
   /**
    * Reset all simulation data.
    */