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

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

@@ -0,0 +1,28 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.hal;
+
+public class LEDJNI extends JNIWrapper {
+  public static final int RADIO_LED_STATE_OFF = 0;
+  public static final int RADIO_LED_STATE_GREEN = 1;
+  public static final int RADIO_LED_STATE_RED = 2;
+  public static final int RADIO_LED_STATE_ORANGE = 3;
+
+  /**
+   * Set the state of the "Radio" LED.
+   *
+   * @param state The state to set the LED to.
+   * @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/java/edu/wpi/first/hal/simulation/RoboRioDataJNI.java

@@ -176,5 +176,14 @@ public static native int registerTeamNumberCallback(
 
   public static native void setComments(String comments);
 
+  public static native int registerRadioLEDStateCallback(
+      NotifyCallback callback, boolean initialNotify);
+
+  public static native void cancelRadioLEDStateCallback(int uid);
+
+  public static native int getRadioLEDState();
+
+  public static native void setRadioLEDState(int state);
+
   public static native void resetData();
 }

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

@@ -83,6 +83,7 @@ void InitializeHAL() {
   InitializeFRCDriverStation();
   InitializeI2C();
   InitializeInterrupts();
+  InitializeLEDs();
   InitializeMain();
   InitializeNotifier();
   InitializeCTREPDP();

hal/src/main/native/athena/HALInitializer.h

@@ -40,6 +40,7 @@ extern void InitializeFRCDriverStation();
 extern void InitializeHAL();
 extern void InitializeI2C();
 extern void InitializeInterrupts();
+extern void InitializeLEDs();
 extern void InitializeMain();
 extern void InitializeNotifier();
 extern void InitializeCTREPDP();

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

@@ -0,0 +1,89 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "hal/LEDs.h"
+
+#include <unistd.h>
+
+#include <fstream>
+
+#include <wpi/fs.h>
+
+#include "hal/Errors.h"
+
+namespace hal::init {
+
+void InitializeLEDs() {
+  int32_t status = 0;
+  HAL_SetRadioLEDState(HAL_RadioLED_kOff, &status);
+}
+}  // namespace hal::init
+
+static const fs::path radioLEDGreenFilePath =
+    "/sys/class/leds/nilrt:wifi:primary/brightness";
+static const fs::path radioLEDRedFilePath =
+    "/sys/class/leds/nilrt:wifi:secondary/brightness";
+
+static const char* onStr = "1";
+static const char* offStr = "0";
+
+extern "C" {
+void HAL_SetRadioLEDState(HAL_RadioLEDState state, int32_t* status) {
+  std::error_code ec;
+  fs::file_t greenFile = fs::OpenFileForWrite(radioLEDGreenFilePath, ec,
+                                              fs::CD_OpenExisting, fs::OF_Text);
+  if (ec) {
+    *status = INCOMPATIBLE_STATE;
+    return;
+  }
+  fs::file_t redFile = fs::OpenFileForWrite(radioLEDRedFilePath, ec,
+                                            fs::CD_OpenExisting, fs::OF_Text);
+  if (ec) {
+    *status = INCOMPATIBLE_STATE;
+    return;
+  }
+
+  write(greenFile, state & HAL_RadioLED_kGreen ? onStr : offStr, 1);
+  write(redFile, state & HAL_RadioLED_kRed ? onStr : offStr, 1);
+
+  fs::CloseFile(greenFile);
+  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/athena/mockdata/RoboRioData.cpp

@@ -30,6 +30,7 @@ DEFINE_CAPI(int32_t, UserFaults3V3, 0)
 DEFINE_CAPI(double, BrownoutVoltage, 6.75)
 DEFINE_CAPI(double, CPUTemp, 45.0)
 DEFINE_CAPI(int32_t, TeamNumber, 0)
+DEFINE_CAPI(HAL_RadioLEDState, RadioLEDState, HAL_RadioLED_kOff);
 
 int32_t HALSIM_RegisterRoboRioSerialNumberCallback(
     HAL_RoboRioStringCallback callback, void* param, HAL_Bool initialNotify) {

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

@@ -0,0 +1,51 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <fmt/core.h>
+
+#include "HALUtil.h"
+#include "edu_wpi_first_hal_LEDJNI.h"
+#include "hal/LEDs.h"
+
+static_assert(edu_wpi_first_hal_LEDJNI_RADIO_LED_STATE_OFF ==
+              HAL_RadioLEDState::HAL_RadioLED_kOff);
+static_assert(edu_wpi_first_hal_LEDJNI_RADIO_LED_STATE_GREEN ==
+              HAL_RadioLEDState::HAL_RadioLED_kGreen);
+static_assert(edu_wpi_first_hal_LEDJNI_RADIO_LED_STATE_RED ==
+              HAL_RadioLEDState::HAL_RadioLED_kRed);
+static_assert(edu_wpi_first_hal_LEDJNI_RADIO_LED_STATE_ORANGE ==
+              HAL_RadioLEDState::HAL_RadioLED_kOrange);
+
+using namespace hal;
+
+extern "C" {
+/*
+ * Class:     edu_wpi_first_hal_LEDJNI
+ * Method:    setRadioLEDState
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_LEDJNI_setRadioLEDState
+  (JNIEnv* env, jclass, jint state)
+{
+  int32_t status = 0;
+  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/cpp/jni/simulation/RoboRioDataJNI.cpp

@@ -983,6 +983,57 @@ Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_setComments
   HALSIM_SetRoboRioComments(commentsJString.c_str(), commentsJString.size());
 }
 
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    registerRadioLEDStateCallback
+ * Signature: (Ljava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_registerRadioLEDStateCallback
+  (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallbackNoIndex(
+      env, callback, initialNotify,
+      &HALSIM_RegisterRoboRioRadioLEDStateCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    cancelRadioLEDStateCallback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_cancelRadioLEDStateCallback
+  (JNIEnv* env, jclass, jint handle)
+{
+  return sim::FreeCallbackNoIndex(env, handle,
+                                  &HALSIM_CancelRoboRioRadioLEDStateCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    getRadioLEDState
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_getRadioLEDState
+  (JNIEnv*, jclass)
+{
+  return HALSIM_GetRoboRioRadioLEDState();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    setRadioLEDState
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_setRadioLEDState
+  (JNIEnv*, jclass, jint value)
+{
+  HALSIM_SetRoboRioRadioLEDState(static_cast<HAL_RadioLEDState>(value));
+}
+
 /*
  * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
  * Method:    resetData

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

@@ -25,6 +25,7 @@
 #include "hal/HALBase.h"
 #include "hal/I2C.h"
 #include "hal/Interrupts.h"
+#include "hal/LEDs.h"
 #include "hal/Main.h"
 #include "hal/Notifier.h"
 #include "hal/PWM.h"

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

@@ -0,0 +1,30 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+#include "hal/Types.h"
+
+HAL_ENUM(HAL_RadioLEDState){HAL_RadioLED_kOff = 0, HAL_RadioLED_kGreen = 1,
+                            HAL_RadioLED_kRed = 2, HAL_RadioLED_kOrange = 3};
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+/**
+ * Set the state of the "Radio" LED.
+ * @param state The state to set the LED to.
+ * @param[out] status the error code, or 0 for success
+ */
+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/include/hal/simulation/RoboRioData.h

@@ -6,6 +6,7 @@
 
 #include <cstddef>
 
+#include "hal/LEDs.h"
 #include "hal/Types.h"
 #include "hal/simulation/NotifyListener.h"
 
@@ -145,16 +146,22 @@ void HALSIM_CancelRoboRioCommentsCallback(int32_t uid);
 size_t HALSIM_GetRoboRioComments(char* buffer, size_t size);
 void HALSIM_SetRoboRioComments(const char* comments, size_t size);
 
-void HALSIM_RegisterRoboRioAllCallbacks(HAL_NotifyCallback callback,
-                                        void* param, HAL_Bool initialNotify);
-
 int32_t HALSIM_RegisterRoboRioCPUTempCallback(HAL_NotifyCallback callback,
                                               void* param,
                                               HAL_Bool initialNotify);
 void HALSIM_CancelRoboRioCPUTempCallback(int32_t uid);
 double HALSIM_GetRoboRioCPUTemp(void);
 void HALSIM_SetRoboRioCPUTemp(double cpuTemp);
 
+int32_t HALSIM_RegisterRoboRioRadioLEDStateCallback(HAL_NotifyCallback callback,
+                                                    void* param,
+                                                    HAL_Bool initialNotify);
+void HALSIM_CancelRoboRioRadioLEDStateCallback(int32_t uid);
+HAL_RadioLEDState HALSIM_GetRoboRioRadioLEDState(void);
+void HALSIM_SetRoboRioRadioLEDState(HAL_RadioLEDState state);
+
+void HALSIM_RegisterRoboRioAllCallbacks(HAL_NotifyCallback callback,
+                                        void* param, HAL_Bool initialNotify);
 #ifdef __cplusplus
 }  // extern "C"
 #endif

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

@@ -0,0 +1,21 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "hal/LEDs.h"
+
+#include "hal/simulation/RoboRioData.h"
+
+namespace hal::init {
+void InitializeLEDs() {}
+}  // namespace hal::init
+
+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"

hal/src/main/native/sim/mockdata/RoboRioData.cpp

@@ -136,6 +136,7 @@ DEFINE_CAPI(int32_t, UserFaults3V3, userFaults3V3)
 DEFINE_CAPI(double, BrownoutVoltage, brownoutVoltage)
 DEFINE_CAPI(double, CPUTemp, cpuTemp)
 DEFINE_CAPI(int32_t, TeamNumber, teamNumber)
+DEFINE_CAPI(HAL_RadioLEDState, RadioLEDState, radioLedState)
 
 int32_t HALSIM_RegisterRoboRioSerialNumberCallback(
     HAL_RoboRioStringCallback callback, void* param, HAL_Bool initialNotify) {
@@ -192,5 +193,6 @@ void HALSIM_RegisterRoboRioAllCallbacks(HAL_NotifyCallback callback,
   REGISTER(userFaults3V3);
   REGISTER(brownoutVoltage);
   REGISTER(cpuTemp);
+  REGISTER(radioLedState);
 }
 }  // extern "C"