[hal, wpilib] Add brownout voltage configuration (#3632)

glass/src/lib/native/include/glass/hardware/RoboRio.h

@@ -32,10 +32,12 @@ class RoboRioModel : public Model {
   virtual DataSource* GetUserButton() = 0;
   virtual DataSource* GetVInVoltageData() = 0;
   virtual DataSource* GetVInCurrentData() = 0;
+  virtual DataSource* GetBrownoutVoltage() = 0;
 
   virtual void SetUserButton(bool val) = 0;
   virtual void SetVInVoltage(double val) = 0;
   virtual void SetVInCurrent(double val) = 0;
+  virtual void SetBrownoutVoltage(double val) = 0;
 };
 
 void DisplayRoboRio(RoboRioModel* model);

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

@@ -32,4 +32,8 @@ public class PowerJNI extends JNIWrapper {
   public static native boolean getUserActive3V3();
 
   public static native int getUserCurrentFaults3V3();
+
+  public static native void setBrownoutVoltage(double voltage);
+
+  public static native double getBrownoutVoltage();
 }

hal/src/main/java/edu/wpi/first/hal/simulation/RoboRioDataJNI.java

@@ -146,5 +146,14 @@ public class RoboRioDataJNI extends JNIWrapper {
 
   public static native void setUserFaults3V3(int userFaults3V3);
 
+  public static native int registerBrownoutVoltageCallback(
+      NotifyCallback callback, boolean initialNotify);
+
+  public static native void cancelBrownoutVoltageCallback(int uid);
+
+  public static native double getBrownoutVoltage();
+
+  public static native void setBrownoutVoltage(double brownoutVoltage);
+
   public static native void resetData();
 }

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

@@ -103,4 +103,22 @@ int32_t HAL_GetUserCurrentFaults3V3(int32_t* status) {
       power->readFaultCounts_OverCurrentFaultCount3V3(status));
 }
 
+void HAL_SetBrownoutVoltage(double voltage, int32_t* status) {
+  initializePower(status);
+  if (voltage < 0) {
+    voltage = 0;
+  }
+  if (voltage > 50) {
+    voltage = 50;
+  }
+  power->writeBrownoutVoltage250mV(static_cast<unsigned char>(voltage * 4),
+                                   status);
+}
+
+double HAL_GetBrownoutVoltage(int32_t* status) {
+  initializePower(status);
+  auto brownout = power->readBrownoutVoltage250mV(status);
+  return brownout / 4.0;
+}
+
 }  // extern "C"

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

@@ -27,6 +27,7 @@ DEFINE_CAPI(HAL_Bool, UserActive3V3, false)
 DEFINE_CAPI(int32_t, UserFaults6V, 0)
 DEFINE_CAPI(int32_t, UserFaults5V, 0)
 DEFINE_CAPI(int32_t, UserFaults3V3, 0)
+DEFINE_CAPI(double, BrownoutVoltage, 6.75)
 
 void HALSIM_RegisterRoboRioAllCallbacks(HAL_NotifyCallback callback,
                                         void* param, HAL_Bool initialNotify) {}

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

@@ -222,4 +222,33 @@ Java_edu_wpi_first_hal_PowerJNI_getUserCurrentFaults3V3
   return val;
 }
 
+/*
+ * Class:     edu_wpi_first_hal_PowerJNI
+ * Method:    setBrownoutVoltage
+ * Signature: (D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_PowerJNI_setBrownoutVoltage
+  (JNIEnv* env, jclass, jdouble brownoutVoltage)
+{
+  int32_t status = 0;
+  HAL_SetBrownoutVoltage(brownoutVoltage, &status);
+  CheckStatus(env, status);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_PowerJNI
+ * Method:    getBrownoutVoltage
+ * Signature: ()D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_PowerJNI_getBrownoutVoltage
+  (JNIEnv* env, jclass)
+{
+  int32_t status = 0;
+  double val = HAL_GetBrownoutVoltage(&status);
+  CheckStatus(env, status);
+  return val;
+}
+
 }  // extern "C"

hal/src/main/native/cpp/jni/simulation/RoboRioDataJNI.cpp

@@ -774,6 +774,57 @@ Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_setUserFaults3V3
   HALSIM_SetRoboRioUserFaults3V3(value);
 }
 
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    registerBrownoutVoltageCallback
+ * Signature: (Ljava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_registerBrownoutVoltageCallback
+  (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallbackNoIndex(
+      env, callback, initialNotify,
+      &HALSIM_RegisterRoboRioBrownoutVoltageCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    cancelBrownoutVoltageCallback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_cancelBrownoutVoltageCallback
+  (JNIEnv* env, jclass, jint handle)
+{
+  return sim::FreeCallbackNoIndex(env, handle,
+                                  &HALSIM_CancelRoboRioBrownoutVoltageCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    getBrownoutVoltage
+ * Signature: ()D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_getBrownoutVoltage
+  (JNIEnv*, jclass)
+{
+  return HALSIM_GetRoboRioBrownoutVoltage();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    setBrownoutVoltage
+ * Signature: (D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_setBrownoutVoltage
+  (JNIEnv*, jclass, jdouble value)
+{
+  HALSIM_SetRoboRioBrownoutVoltage(value);
+}
+
 /*
  * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
  * Method:    resetData

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

@@ -115,6 +115,24 @@ HAL_Bool HAL_GetUserActive3V3(int32_t* status);
  * @return the number of 3V3 fault counts
  */
 int32_t HAL_GetUserCurrentFaults3V3(int32_t* status);
+
+/**
+ * Get the current brownout voltage seting.
+ *
+ * Note that this only does anything on the roboRIO 2.
+ * On the roboRIO it is a no-op.
+ *
+ * @return The brownout voltage
+ */
+double HAL_GetBrownoutVoltage(int32_t* status);
+
+/**
+ * Set the voltage the roborio will brownout and disable all outputs.
+ *
+ * @param brownoutVoltage The brownout voltage
+ */
+void HAL_SetBrownoutVoltage(double voltage, int32_t* status);
+
 #ifdef __cplusplus
 }  // extern "C"
 #endif

hal/src/main/native/include/hal/simulation/RoboRioData.h

@@ -115,6 +115,12 @@ void HALSIM_CancelRoboRioUserFaults3V3Callback(int32_t uid);
 int32_t HALSIM_GetRoboRioUserFaults3V3(void);
 void HALSIM_SetRoboRioUserFaults3V3(int32_t userFaults3V3);
 
+int32_t HALSIM_RegisterRoboRioBrownoutVoltageCallback(
+    HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify);
+void HALSIM_CancelRoboRioBrownoutVoltageCallback(int32_t uid);
+double HALSIM_GetRoboRioBrownoutVoltage(void);
+void HALSIM_SetRoboRioBrownoutVoltage(double brownoutVoltage);
+
 void HALSIM_RegisterRoboRioAllCallbacks(HAL_NotifyCallback callback,
                                         void* param, HAL_Bool initialNotify);
 

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

@@ -56,4 +56,10 @@ HAL_Bool HAL_GetUserActive3V3(int32_t* status) {
 int32_t HAL_GetUserCurrentFaults3V3(int32_t* status) {
   return SimRoboRioData->userFaults3V3;
 }
+void HAL_SetBrownoutVoltage(double voltage, int32_t* status) {
+  SimRoboRioData->brownoutVoltage = voltage;
+}
+double HAL_GetBrownoutVoltage(int32_t* status) {
+  return SimRoboRioData->brownoutVoltage;
+}
 }  // extern "C"

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

@@ -31,6 +31,7 @@ void RoboRioData::ResetData() {
   userFaults6V.Reset(0);
   userFaults5V.Reset(0);
   userFaults3V3.Reset(0);
+  brownoutVoltage.Reset(6.75);
 }
 
 extern "C" {
@@ -57,6 +58,7 @@ DEFINE_CAPI(HAL_Bool, UserActive3V3, userActive3V3)
 DEFINE_CAPI(int32_t, UserFaults6V, userFaults6V)
 DEFINE_CAPI(int32_t, UserFaults5V, userFaults5V)
 DEFINE_CAPI(int32_t, UserFaults3V3, userFaults3V3)
+DEFINE_CAPI(double, BrownoutVoltage, brownoutVoltage)
 
 #define REGISTER(NAME) \
   SimRoboRioData->NAME.RegisterCallback(callback, param, initialNotify)
@@ -78,5 +80,6 @@ void HALSIM_RegisterRoboRioAllCallbacks(HAL_NotifyCallback callback,
   REGISTER(userFaults6V);
   REGISTER(userFaults5V);
   REGISTER(userFaults3V3);
+  REGISTER(brownoutVoltage);
 }
 }  // extern "C"

hal/src/main/native/sim/mockdata/RoboRioDataInternal.h

@@ -24,6 +24,7 @@ class RoboRioData {
   HAL_SIMDATAVALUE_DEFINE_NAME(UserFaults6V)
   HAL_SIMDATAVALUE_DEFINE_NAME(UserFaults5V)
   HAL_SIMDATAVALUE_DEFINE_NAME(UserFaults3V3)
+  HAL_SIMDATAVALUE_DEFINE_NAME(BrownoutVoltage)
 
  public:
   SimDataValue<HAL_Bool, HAL_MakeBoolean, GetFPGAButtonName> fpgaButton{false};
@@ -46,6 +47,8 @@ class RoboRioData {
   SimDataValue<int32_t, HAL_MakeInt, GetUserFaults6VName> userFaults6V{0};
   SimDataValue<int32_t, HAL_MakeInt, GetUserFaults5VName> userFaults5V{0};
   SimDataValue<int32_t, HAL_MakeInt, GetUserFaults3V3Name> userFaults3V3{0};
+  SimDataValue<double, HAL_MakeDouble, GetBrownoutVoltageName> brownoutVoltage{
+      6.75};
 
   virtual void ResetData();
 };

simulation/halsim_gui/src/main/native/cpp/RoboRioSimGui.cpp

@@ -31,6 +31,7 @@ HALSIMGUI_DATASOURCE_DOUBLE(RoboRioUserVoltage3V3, "Rio 3.3V Voltage");
 HALSIMGUI_DATASOURCE_DOUBLE(RoboRioUserCurrent3V3, "Rio 3.3V Current");
 HALSIMGUI_DATASOURCE_BOOLEAN(RoboRioUserActive3V3, "Rio 3.3V Active");
 HALSIMGUI_DATASOURCE_INT(RoboRioUserFaults3V3, "Rio 3.3V Faults");
+HALSIMGUI_DATASOURCE_DOUBLE(RoboRioBrownoutVoltage, "Rio Brownout Voltage");
 
 class RoboRioUser6VRailSimModel : public glass::RoboRioRailModel {
  public:
@@ -108,10 +109,16 @@ class RoboRioSimModel : public glass::RoboRioModel {
   glass::DataSource* GetUserButton() override { return &m_userButton; }
   glass::DataSource* GetVInVoltageData() override { return &m_vInVoltage; }
   glass::DataSource* GetVInCurrentData() override { return &m_vInCurrent; }
+  glass::DataSource* GetBrownoutVoltage() override {
+    return &m_brownoutVoltage;
+  }
 
   void SetUserButton(bool val) override { HALSIM_SetRoboRioFPGAButton(val); }
   void SetVInVoltage(double val) override { HALSIM_SetRoboRioVInVoltage(val); }
   void SetVInCurrent(double val) override { HALSIM_SetRoboRioVInCurrent(val); }
+  void SetBrownoutVoltage(double val) override {
+    HALSIM_SetRoboRioBrownoutVoltage(val);
+  }
 
  private:
   RoboRioFPGAButtonSource m_userButton;
@@ -120,6 +127,7 @@ class RoboRioSimModel : public glass::RoboRioModel {
   RoboRioUser6VRailSimModel m_user6VRail;
   RoboRioUser5VRailSimModel m_user5VRail;
   RoboRioUser3V3RailSimModel m_user3V3Rail;
+  RoboRioBrownoutVoltageSource m_brownoutVoltage;
 };
 }  // namespace
 

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

@@ -159,6 +159,19 @@ int RobotController::GetFaultCount6V() {
   return retVal;
 }
 
+units::volt_t RobotController::GetBrownoutVoltage() {
+  int32_t status = 0;
+  double retVal = HAL_GetBrownoutVoltage(&status);
+  FRC_CheckErrorStatus(status, "{}", "GetBrownoutVoltage");
+  return units::volt_t(retVal);
+}
+
+void RobotController::SetBrownoutVoltage(units::volt_t brownoutVoltage) {
+  int32_t status = 0;
+  HAL_SetBrownoutVoltage(brownoutVoltage.to<double>(), &status);
+  FRC_CheckErrorStatus(status, "{}", "SetBrownoutVoltage");
+}
+
 CANStatus RobotController::GetCANStatus() {
   int32_t status = 0;
   float percentBusUtilization = 0;

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

@@ -267,6 +267,23 @@ void RoboRioSim::SetUserFaults3V3(int userFaults3V3) {
   HALSIM_SetRoboRioUserFaults3V3(userFaults3V3);
 }
 
+std::unique_ptr<CallbackStore> RoboRioSim::RegisterBrownoutVoltageCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelRoboRioBrownoutVoltageCallback);
+  store->SetUid(HALSIM_RegisterRoboRioBrownoutVoltageCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+units::volt_t RoboRioSim::GetBrownoutVoltage() {
+  return units::volt_t(HALSIM_GetRoboRioBrownoutVoltage());
+}
+
+void RoboRioSim::SetBrownoutVoltage(units::volt_t vInVoltage) {
+  HALSIM_SetRoboRioBrownoutVoltage(vInVoltage.to<double>());
+}
+
 void RoboRioSim::ResetData() {
   HALSIM_ResetRoboRioData();
 }

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

@@ -188,6 +188,28 @@ class RobotController {
    */
   static int GetFaultCount6V();
 
+  /**
+   * Get the current brownout voltage seting.
+   *
+   * @return The brownout voltage
+   */
+  static units::volt_t GetBrownoutVoltage();
+
+  /**
+   * Set the voltage the roborio will brownout and disable all outputs.
+   *
+   * Note that this only does anything on the roboRIO 2.
+   * On the roboRIO it is a no-op.
+   *
+   * @param brownoutVoltage The brownout voltage
+   */
+  static void SetBrownoutVoltage(units::volt_t brownoutVoltage);
+
+  /**
+   * Get the current status of the CAN bus.
+   *
+   * @return The status of the CAN bus
+   */
   static CANStatus GetCANStatus();
 };
 

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

@@ -394,6 +394,30 @@ class RoboRioSim {
    */
   static void SetUserFaults3V3(int userFaults3V3);
 
+  /**
+   * Register a callback to be run whenever the brownout voltage changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] static std::unique_ptr<CallbackStore>
+  RegisterBrownoutVoltageCallback(NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Measure the brownout voltage.
+   *
+   * @return the brownout voltage
+   */
+  static units::volt_t GetBrownoutVoltage();
+
+  /**
+   * Define the brownout voltage.
+   *
+   * @param vInVoltage the new voltage
+   */
+  static void SetBrownoutVoltage(units::volt_t brownoutVoltage);
+
   /**
    * Reset all simulation data.
    */

wpilibc/src/test/native/cpp/simulation/RoboRioSimTest.cpp

@@ -60,6 +60,21 @@ TEST(RoboRioSimTest, SetVin) {
   EXPECT_EQ(kTestCurrent, RobotController::GetInputCurrent());
 }
 
+TEST(RoboRioSimTest, SetBrownout) {
+  RoboRioSim::ResetData();
+
+  DoubleCallback voltageCallback;
+  auto voltageCb = RoboRioSim::RegisterBrownoutVoltageCallback(
+      voltageCallback.GetCallback(), false);
+  constexpr double kTestVoltage = 1.91;
+
+  RoboRioSim::SetBrownoutVoltage(units::volt_t{kTestVoltage});
+  EXPECT_TRUE(voltageCallback.WasTriggered());
+  EXPECT_EQ(kTestVoltage, voltageCallback.GetLastValue());
+  EXPECT_EQ(kTestVoltage, RoboRioSim::GetBrownoutVoltage().to<double>());
+  EXPECT_EQ(kTestVoltage, RobotController::GetBrownoutVoltage().to<double>());
+}
+
 TEST(RoboRioSimTest, Set6V) {
   RoboRioSim::ResetData();
 

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

@@ -213,6 +213,26 @@ public final class RobotController {
     return PowerJNI.getUserCurrentFaults6V();
   }
 
+  /**
+   * Get the current brownout voltage seting.
+   *
+   * @return The brownout voltage
+   */
+  public static double getBrownoutVoltage() {
+    return PowerJNI.getBrownoutVoltage();
+  }
+
+  /**
+   * Set the voltage the roborio will brownout and disable all outputs.
+   *
+   * <p>Note that this only does anything on the roboRIO 2. On the roboRIO it is a no-op.
+   *
+   * @param brownoutVoltage The brownout voltage
+   */
+  public static void setBrownoutVoltage(double brownoutVoltage) {
+    PowerJNI.setBrownoutVoltage(brownoutVoltage);
+  }
+
   /**
    * Get the current status of the CAN bus.
    *

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

@@ -498,6 +498,39 @@ public final class RoboRioSim {
     RoboRioDataJNI.setUserFaults3V3(userFaults3V3);
   }
 
+  /**
+   * Register a callback to be run whenever the Brownout voltage 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 registerBrownoutVoltageCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = RoboRioDataJNI.registerBrownoutVoltageCallback(callback, initialNotify);
+    return new CallbackStore(uid, RoboRioDataJNI::cancelBrownoutVoltageCallback);
+  }
+
+  /**
+   * Measure the Brownout voltage.
+   *
+   * @return the Brownout voltage
+   */
+  public static double getBrownoutVoltage() {
+    return RoboRioDataJNI.getBrownoutVoltage();
+  }
+
+  /**
+   * Define the Brownout voltage.
+   *
+   * @param vInVoltage the new voltage
+   */
+  @SuppressWarnings("ParameterName")
+  public static void setBrownoutVoltage(double vInVoltage) {
+    RoboRioDataJNI.setBrownoutVoltage(vInVoltage);
+  }
+
   /** Reset all simulation data. */
   public static void resetData() {
     RoboRioDataJNI.resetData();

wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/RoboRioSimTest.java

@@ -62,6 +62,23 @@ public class RoboRioSimTest {
     }
   }
 
+  @Test
+  void testSetBrownout() {
+    RoboRioSim.resetData();
+
+    DoubleCallback voltageCallback = new DoubleCallback();
+    try (CallbackStore voltageCb =
+        RoboRioSim.registerBrownoutVoltageCallback(voltageCallback, false)) {
+      final double kTestVoltage = 1.91;
+
+      RoboRioSim.setBrownoutVoltage(kTestVoltage);
+      assertTrue(voltageCallback.wasTriggered());
+      assertEquals(kTestVoltage, voltageCallback.getSetValue());
+      assertEquals(kTestVoltage, RoboRioSim.getBrownoutVoltage());
+      assertEquals(kTestVoltage, RobotController.getBrownoutVoltage());
+    }
+  }
+
   @Test
   void test6V() {
     RoboRioSim.resetData();