Add function to read state

hal/src/main/java/edu/wpi/first/hal/LEDJNI.java

@@ -17,4 +17,12 @@ public class LEDJNI extends JNIWrapper {
    * @see "HAL_SetRadioLEDState"
    */
   public static native void setRadioLEDState(int state);
+
+  /**
+   * Get the state of the "Radio" LED.
+   *
+   * @return The state of the LED.
+   * @see "HAL_GetRadioLEDState"
+   */
+  public static native int getRadioLEDState();
 }

hal/src/main/native/athena/LEDs.cpp

@@ -51,4 +51,39 @@ void HAL_SetRadioLEDState(HAL_RadioLEDState state, int32_t* status) {
   fs::CloseFile(redFile);
 }
 
+bool ReadStateFromFile(fs::path path, int32_t* status) {
+  std::error_code ec;
+  fs::file_t file = fs::OpenFileForRead(path, ec, fs::OF_Text);
+  if (ec) {
+    *status = INCOMPATIBLE_STATE;
+    return false;
+  }
+  // We only need to read one byte because the file won't have leading zeros.
+  char buf[1]{};
+  size_t count = read(file, buf, 1);
+  if (count == 0) {
+    *status = INCOMPATIBLE_STATE;
+    return false;
+  }
+  // If the brightness is not zero, the LED is on.
+  return buf[0] != '0';
+}
+
+HAL_RadioLEDState HAL_GetRadioLEDState(int32_t* status) {
+  bool green = ReadStateFromFile(radioLEDGreenFilePath, status);
+  bool red = ReadStateFromFile(radioLEDRedFilePath, status);
+  if (*status == 0) {
+    if (green && red) {
+      return HAL_RadioLED_kOrange;
+    } else if (green) {
+      return HAL_RadioLED_kGreen;
+    } else if (red) {
+      return HAL_RadioLED_kRed;
+    } else {
+      return HAL_RadioLED_kOff;
+    }
+  } else {
+    return HAL_RadioLED_kOff;
+  }
+}
 }  // extern "C"

hal/src/main/native/cpp/jni/LEDJNI.cpp

@@ -33,4 +33,19 @@ Java_edu_wpi_first_hal_LEDJNI_setRadioLEDState
   HAL_SetRadioLEDState(static_cast<HAL_RadioLEDState>(state), &status);
   CheckStatus(env, status);
 }
+
+/*
+ * Class:     edu_wpi_first_hal_LEDJNI
+ * Method:    getRadioLEDState
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_LEDJNI_getRadioLEDState
+  (JNIEnv* env, jclass)
+{
+  int32_t status = 0;
+  auto retVal = HAL_GetRadioLEDState(&status);
+  CheckStatus(env, status);
+  return retVal;
+}
 }  // extern "C"

hal/src/main/native/include/hal/LEDs.h

@@ -18,6 +18,13 @@ extern "C" {
  */
 void HAL_SetRadioLEDState(HAL_RadioLEDState state, int32_t* status);
 
+/**
+ * Get the state of the "Radio" LED.
+ *
+ * @param[out] status the error code, or 0 for success
+ * @return The state of the LED.
+ */
+HAL_RadioLEDState HAL_GetRadioLEDState(int32_t* status);
 #ifdef __cplusplus
 }  // extern "C"
 #endif

hal/src/main/native/sim/LEDs.cpp

@@ -15,5 +15,7 @@ extern "C" {
 void HAL_SetRadioLEDState(HAL_RadioLEDState state, int32_t* status) {
   HALSIM_SetRoboRioRadioLEDState(state);
 }
-
+HAL_RadioLEDState HAL_GetRadioLEDState(int32_t* status) {
+  return HALSIM_GetRoboRioRadioLEDState();
+}
 }  // extern "C"

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

@@ -248,6 +248,13 @@ void RobotController::SetRadioLEDState(RadioLEDState state) {
   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/include/frc/RobotController.h

@@ -284,6 +284,15 @@ class RobotController {
    */
   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/test/native/cpp/simulation/RoboRioSimTest.cpp

@@ -285,13 +285,15 @@ TEST(RoboRioSimTest, SetRadioLEDState) {
   EXPECT_TRUE(callback.WasTriggered());
   EXPECT_EQ(RadioLEDState::kGreen, callback.GetLastValue());
   EXPECT_EQ(RadioLEDState::kGreen, RoboRioSim::GetRadioLEDState());
+  EXPECT_EQ(RadioLEDState::kGreen, RobotController::GetRadioLEDState());
 
   callback.Reset();
 
   RoboRioSim::SetRadioLEDState(RadioLEDState::kOrange);
   EXPECT_TRUE(callback.WasTriggered());
   EXPECT_EQ(RadioLEDState::kOrange, callback.GetLastValue());
   EXPECT_EQ(RadioLEDState::kOrange, RoboRioSim::GetRadioLEDState());
+  EXPECT_EQ(RadioLEDState::kOrange, RobotController::GetRadioLEDState());
 }
 
 }  // namespace frc::sim

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

@@ -366,6 +366,16 @@ 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/test/java/edu/wpi/first/wpilibj/simulation/RoboRioSimTest.java

@@ -292,13 +292,15 @@ void testRadioLEDState() {
       assertTrue(callback.wasTriggered());
       assertEquals(RadioLEDState.kGreen.value, callback.getSetValue());
       assertEquals(RadioLEDState.kGreen, RoboRioSim.getRadioLEDState());
+      assertEquals(RadioLEDState.kGreen, RobotController.getRadioLEDState());
 
       callback.reset();
 
       RoboRioSim.setRadioLEDState(RadioLEDState.kOrange);
       assertTrue(callback.wasTriggered());
       assertEquals(RadioLEDState.kOrange.value, callback.getSetValue());
       assertEquals(RadioLEDState.kOrange, RoboRioSim.getRadioLEDState());
+      assertEquals(RadioLEDState.kOrange, RobotController.getRadioLEDState());
     }
   }
 }