[hal, wpilib] Add DS OpMode support

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

@@ -139,6 +139,13 @@ public static void getControlWord(ControlWord controlWord) {
         ((word >> 5) & 1) != 0);
   }
 
+  /**
+   * Gets the current operating mode of the driver station.
+   *
+   * @return the current operating mode
+   */
+  public static native String getOpMode();
+
   /**
    * Gets the current alliance station ID.
    *

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

@@ -77,6 +77,10 @@ public static native int registerMatchTimeCallback(
 
   public static native void setMatchTime(double matchTime);
 
+  public static native String getOpMode();
+
+  public static native void setOpMode(String opMode);
+
   public static native void setJoystickAxes(byte joystickNum, float[] axesArray);
 
   public static native void setJoystickPOVs(byte joystickNum, short[] povsArray);

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

@@ -136,6 +136,21 @@ Java_edu_wpi_first_hal_DriverStationJNI_nativeGetControlWord
   return retVal;
 }
 
+/*
+ * Class:     edu_wpi_first_hal_DriverStationJNI
+ * Method:    getOpMode
+ * Signature: ()java/lang/String;
+ */
+JNIEXPORT jstring JNICALL
+Java_edu_wpi_first_hal_DriverStationJNI_getOpMode
+  (JNIEnv* env, jclass)
+{
+  char buf[128];
+  int32_t len = 128;
+  HAL_GetOpMode(buf, &len);
+  return MakeJString(env, std::string_view{buf, static_cast<size_t>(len)});
+}
+
 /*
  * Class:     edu_wpi_first_hal_DriverStationJNI
  * Method:    nativeGetAllianceStation

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

@@ -424,6 +424,33 @@ Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_setMatchTime
   HALSIM_SetDriverStationMatchTime(value);
 }
 
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    getOpMode
+ * Signature: ()java/lang/String;
+ */
+JNIEXPORT jstring JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_getOpMode
+  (JNIEnv* env, jclass)
+{
+  char buf[128];
+  int32_t len = 128;
+  HALSIM_GetOpMode(buf, &len);
+  return MakeJString(env, std::string_view{buf, static_cast<size_t>(len)});
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    setOpMode
+ * Signature: (java/lang/String;)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_setOpMode
+  (JNIEnv* env, jclass, jstring value)
+{
+  HALSIM_SetOpMode(JStringRef{env, value}.c_str());
+}
+
 /*
  * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
  * Method:    setJoystickAxes

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

@@ -66,6 +66,15 @@ int32_t HAL_SendConsoleLine(const char* line);
  */
 int32_t HAL_GetControlWord(HAL_ControlWord* controlWord);
 
+/**
+ * Gets the current operating mode of the driver station.
+ *
+ * @param buf buffer for operating mode string (out)
+ * @param len length of buffer (in), length of string (out)
+ * @return the error code, or 0 for success
+ */
+int32_t HAL_GetOpMode(char* buf, int32_t* len);
+
 /**
  * Gets the current alliance station ID.
  *

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

@@ -12,6 +12,8 @@
 #include "hal/Types.h"
 #include "hal/simulation/NotifyListener.h"
 
+typedef void (*HAL_OpModeCallback)(const char* name, void* param,
+                                   const char* opMode);
 typedef void (*HAL_JoystickAxesCallback)(const char* name, void* param,
                                          int32_t joystickNum,
                                          const HAL_JoystickAxes* axes);
@@ -88,6 +90,12 @@ void HALSIM_CancelDriverStationMatchTimeCallback(int32_t uid);
 double HALSIM_GetDriverStationMatchTime(void);
 void HALSIM_SetDriverStationMatchTime(double matchTime);
 
+int32_t HALSIM_RegisterOpModeCallback(HAL_OpModeCallback callback, void* param,
+                                      HAL_Bool initialNotify);
+void HALSIM_CancelOpModeCallback(int32_t uid);
+void HALSIM_GetOpMode(char* buf, int32_t* len);
+void HALSIM_SetOpMode(const char* opMode);
+
 int32_t HALSIM_RegisterJoystickAxesCallback(int32_t joystickNum,
                                             HAL_JoystickAxesCallback callback,
                                             void* param,

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

@@ -45,6 +45,7 @@ struct JoystickDataCache {
   HAL_AllianceStationID allianceStation;
   double matchTime;
   HAL_ControlWord controlWord;
+  std::string opMode;
 };
 static_assert(std::is_standard_layout_v<JoystickDataCache>);
 // static_assert(std::is_trivial_v<JoystickDataCache>);
@@ -77,6 +78,8 @@ void JoystickDataCache::Update() {
   tmpControlWord.fmsAttached = SimDriverStationData->fmsAttached;
   tmpControlWord.dsAttached = SimDriverStationData->dsAttached;
   this->controlWord = tmpControlWord;
+
+  opMode = SimDriverStationData->GetOpMode();
 }
 
 #define CHECK_JOYSTICK_NUMBER(stickNum)                  \
@@ -227,6 +230,18 @@ int32_t HAL_GetControlWord(HAL_ControlWord* controlWord) {
   return 0;
 }
 
+int32_t HAL_GetOpMode(char* buf, int32_t* len) {
+  if (gShutdown) {
+    return INCOMPATIBLE_STATE;
+  }
+  std::scoped_lock lock{driverStation->cacheMutex};
+  *len = std::min(static_cast<size_t>(*len), currentRead->opMode.length());
+  if (*len != 0) {
+    std::memcpy(buf, currentRead->opMode.data(), *len);
+  }
+  return 0;
+}
+
 HAL_AllianceStationID HAL_GetAllianceStation(int32_t* status) {
   if (gShutdown) {
     return HAL_AllianceStationID_kUnknown;

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

@@ -55,6 +55,31 @@ void DriverStationData::ResetData() {
   m_newDataCallbacks.Reset();
 }
 
+int32_t DriverStationData::RegisterOpModeCallback(HAL_OpModeCallback callback,
+                                                  void* param,
+                                                  HAL_Bool initialNotify) {
+  std::scoped_lock lock(m_opModeMutex);
+  int32_t uid = m_opModeCallbacks.Register(callback, param);
+  if (initialNotify) {
+    callback(DriverStationData::GetOpModeName(), param, m_opMode.c_str());
+  }
+  return uid;
+}
+
+void DriverStationData::CancelOpModeCallback(int32_t uid) {
+  m_opModeCallbacks.Cancel(uid);
+}
+
+std::string DriverStationData::GetOpMode() {
+  std::scoped_lock lock{m_opModeMutex};
+  return m_opMode;
+}
+
+void DriverStationData::SetOpMode(std::string_view opMode) {
+  std::scoped_lock lock{m_opModeMutex};
+  m_opMode = opMode;
+}
+
 #define DEFINE_CPPAPI_CALLBACKS(name, data, data2)                             \
   int32_t DriverStationData::RegisterJoystick##name##Callback(                 \
       int32_t joystickNum, HAL_Joystick##name##Callback callback, void* param, \
@@ -415,6 +440,28 @@ DEFINE_CAPI(HAL_Bool, DsAttached, dsAttached)
 DEFINE_CAPI(HAL_AllianceStationID, AllianceStationId, allianceStationId)
 DEFINE_CAPI(double, MatchTime, matchTime)
 
+int32_t HALSIM_RegisterOpModeCallback(HAL_OpModeCallback callback, void* param,
+                                      HAL_Bool initialNotify) {
+  return SimDriverStationData->RegisterOpModeCallback(callback, param,
+                                                      initialNotify);
+}
+
+void HALSIM_CancelOpModeCallback(int32_t uid) {
+  SimDriverStationData->CancelOpModeCallback(uid);
+}
+
+void HALSIM_GetOpMode(char* buf, int32_t* len) {
+  std::string str = SimDriverStationData->GetOpMode();
+  *len = std::min(static_cast<size_t>(*len), str.length());
+  if (*len != 0) {
+    std::memcpy(buf, str.data(), *len);
+  }
+}
+
+void HALSIM_SetOpMode(const char* opMode) {
+  SimDriverStationData->SetOpMode(opMode);
+}
+
 #undef DEFINE_CAPI
 #define DEFINE_CAPI(name, data)                                                \
   int32_t HALSIM_RegisterJoystick##name##Callback(                             \

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

@@ -5,6 +5,8 @@
 #pragma once
 
 #include <memory>
+#include <string>
+#include <string_view>
 
 #include <wpi/spinlock.h>
 
@@ -23,6 +25,7 @@ class DriverStationData {
   HAL_SIMDATAVALUE_DEFINE_NAME(DsAttached)
   HAL_SIMDATAVALUE_DEFINE_NAME(AllianceStationId)
   HAL_SIMDATAVALUE_DEFINE_NAME(MatchTime)
+  HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(OpMode)
   HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(JoystickAxes)
   HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(JoystickPOVs)
   HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(JoystickButtons)
@@ -40,6 +43,12 @@ class DriverStationData {
   DriverStationData();
   void ResetData();
 
+  int32_t RegisterOpModeCallback(HAL_OpModeCallback callback, void* param,
+                                 HAL_Bool initialNotify);
+  void CancelOpModeCallback(int32_t uid);
+  std::string GetOpMode();
+  void SetOpMode(std::string_view opMode);
+
   int32_t RegisterJoystickAxesCallback(int32_t joystickNum,
                                        HAL_JoystickAxesCallback callback,
                                        void* param, HAL_Bool initialNotify);
@@ -129,6 +138,7 @@ class DriverStationData {
   SimDataValue<double, HAL_MakeDouble, GetMatchTimeName> matchTime{-1.0};
 
  private:
+  SimCallbackRegistry<HAL_OpModeCallback, GetOpModeName> m_opModeCallbacks;
   SimCallbackRegistry<HAL_JoystickAxesCallback, GetJoystickAxesName>
       m_joystickAxesCallbacks;
   SimCallbackRegistry<HAL_JoystickPOVsCallback, GetJoystickPOVsName>
@@ -163,6 +173,9 @@ class DriverStationData {
 
   wpi::spinlock m_matchInfoMutex;
   HAL_MatchInfo m_matchInfo;
+
+  wpi::spinlock m_opModeMutex;
+  std::string m_opMode;
 };
 extern DriverStationData* SimDriverStationData;
 }  // namespace hal

hal/src/main/native/systemcore/FRCDriverStation.cpp

@@ -55,6 +55,7 @@ struct JoystickDataCache {
   HAL_AllianceStationID allianceStation;
   float matchTime;
   HAL_ControlWord controlWord;
+  std::string opMode;
 };
 static_assert(std::is_standard_layout_v<JoystickDataCache>);
 // static_assert(std::is_trivial_v<JoystickDataCache>);
@@ -213,10 +214,10 @@ void JoystickDataCache::Update(const mrc::ControlData& data) {
   controlWord.dsAttached = data.ControlWord.DsConnected;
   controlWord.eStop = data.ControlWord.EStop;
 
-  auto mode = data.GetOpMode();
-  if (mode == "Test") {
+  opMode = data.GetOpMode();
+  if (opMode == "Test") {
     controlWord.test = true;
-  } else if (mode == "Auton") {
+  } else if (opMode == "Auton") {
     controlWord.autonomous = true;
   }
 
@@ -433,6 +434,15 @@ int32_t HAL_GetControlWord(HAL_ControlWord* controlWord) {
   return 0;
 }
 
+int32_t HAL_GetOpMode(char* buf, int32_t* len) {
+  std::scoped_lock lock{cacheMutex};
+  *len = std::min(static_cast<size_t>(*len), currentRead->opMode.length());
+  if (*len != 0) {
+    std::memcpy(buf, currentRead->opMode.data(), *len);
+  }
+  return 0;
+}
+
 int32_t HAL_GetJoystickAxes(int32_t joystickNum, HAL_JoystickAxes* axes) {
   CHECK_JOYSTICK_NUMBER(joystickNum);
   std::scoped_lock lock{cacheMutex};

wpilibc/src/main/native/cpp/DriverStation.cpp

@@ -461,6 +461,21 @@ bool DriverStation::IsEStopped() {
   return controlWord.eStop;
 }
 
+std::string DriverStation::GetMode() {
+  char buf[128];
+  int32_t len = 128;
+  HAL_GetOpMode(buf, &len);
+  return {buf, static_cast<size_t>(len)};
+}
+
+bool DriverStation::IsOpMode(std::string_view mode) {
+  return GetMode() == mode;
+}
+
+bool DriverStation::IsOpModeEnabled(std::string_view mode) {
+  return IsEnabled() && IsOpMode(mode);
+}
+
 bool DriverStation::IsAutonomous() {
   HAL_ControlWord controlWord;
   HAL_GetControlWord(&controlWord);

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

@@ -194,6 +194,30 @@ class DriverStation final {
    */
   static bool IsEStopped();
 
+  /**
+   * Get the current operating mode.
+   *
+   * @return Operating mode
+   */
+  static std::string GetMode();
+
+  /**
+   * Check to see if the current operating mode is a particular value.
+   *
+   * @param mode operating mode
+   * @return True if that mode is the current mode
+   */
+  static bool IsOpMode(std::string_view mode);
+
+  /**
+   * Check to see if the current operating mode is a particular value and the
+   * robot is enabled.
+   *
+   * @param mode operating mode
+   * @return True if that mode is the current mode and the robot is enabled
+   */
+  static bool IsOpModeEnabled(std::string_view mode);
+
   /**
    * Check if the DS is commanding autonomous mode.
    *