Publishes match specific data and other FMS info to NT (#874)

This is so products like SB can present the current setup.

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

@@ -13,6 +13,10 @@
 #include <HAL/Power.h>
 #include <HAL/cpp/Log.h>
 #include <llvm/SmallString.h>
+#include <llvm/StringRef.h>
+#include <networktables/NetworkTable.h>
+#include <networktables/NetworkTableEntry.h>
+#include <networktables/NetworkTableInstance.h>
 
 #include "AnalogInput.h"
 #include "MotorSafetyHelper.h"
@@ -28,6 +32,42 @@ struct MatchInfoData {
   int replayNumber = 0;
   DriverStation::MatchType matchType = DriverStation::MatchType::kNone;
 };
+
+class MatchDataSender {
+ public:
+  std::shared_ptr<nt::NetworkTable> table;
+  nt::NetworkTableEntry typeMetadata;
+  nt::NetworkTableEntry gameSpecificMessage;
+  nt::NetworkTableEntry eventName;
+  nt::NetworkTableEntry matchNumber;
+  nt::NetworkTableEntry replayNumber;
+  nt::NetworkTableEntry matchType;
+  nt::NetworkTableEntry alliance;
+  nt::NetworkTableEntry station;
+  nt::NetworkTableEntry controlWord;
+
+  MatchDataSender() {
+    table = nt::NetworkTableInstance::GetDefault().GetTable("FMSInfo");
+    typeMetadata = table->GetEntry(".type");
+    typeMetadata.ForceSetString("FMSInfo");
+    gameSpecificMessage = table->GetEntry("GameSpecificMessage");
+    gameSpecificMessage.ForceSetString("");
+    eventName = table->GetEntry("EventName");
+    eventName.ForceSetString("");
+    matchNumber = table->GetEntry("MatchNumber");
+    matchNumber.ForceSetDouble(0);
+    replayNumber = table->GetEntry("ReplayNumber");
+    replayNumber.ForceSetDouble(0);
+    matchType = table->GetEntry("MatchType");
+    matchType.ForceSetDouble(0);
+    alliance = table->GetEntry("IsRedAlliance");
+    alliance.ForceSetBoolean(true);
+    station = table->GetEntry("StationNumber");
+    station.ForceSetDouble(1);
+    controlWord = table->GetEntry("FMSControlData");
+    controlWord.ForceSetDouble(0);
+  }
+};
 }  // namespace frc
 
 using namespace frc;
@@ -640,6 +680,65 @@ double DriverStation::GetBatteryVoltage() const {
   return voltage;
 }
 
+void DriverStation::SendMatchData() {
+  int32_t status = 0;
+  HAL_AllianceStationID alliance = HAL_GetAllianceStation(&status);
+  bool isRedAlliance = false;
+  int stationNumber = 1;
+  switch (alliance) {
+    case HAL_AllianceStationID::HAL_AllianceStationID_kBlue1:
+      isRedAlliance = false;
+      stationNumber = 1;
+      break;
+    case HAL_AllianceStationID::HAL_AllianceStationID_kBlue2:
+      isRedAlliance = false;
+      stationNumber = 2;
+      break;
+    case HAL_AllianceStationID::HAL_AllianceStationID_kBlue3:
+      isRedAlliance = false;
+      stationNumber = 3;
+      break;
+    case HAL_AllianceStationID::HAL_AllianceStationID_kRed1:
+      isRedAlliance = true;
+      stationNumber = 1;
+      break;
+    case HAL_AllianceStationID::HAL_AllianceStationID_kRed2:
+      isRedAlliance = true;
+      stationNumber = 2;
+      break;
+    default:
+      isRedAlliance = true;
+      stationNumber = 3;
+      break;
+  }
+
+  MatchInfoData tmpDataStore;
+  {
+    std::lock_guard<wpi::mutex> lock(m_cacheDataMutex);
+    tmpDataStore = *m_matchInfo;
+  }
+
+  m_matchDataSender->alliance.SetBoolean(isRedAlliance);
+  m_matchDataSender->station.SetDouble(stationNumber);
+  m_matchDataSender->eventName.SetString(tmpDataStore.eventName);
+  m_matchDataSender->gameSpecificMessage.SetString(
+      tmpDataStore.gameSpecificMessage);
+  m_matchDataSender->matchNumber.SetDouble(tmpDataStore.matchNumber);
+  m_matchDataSender->replayNumber.SetDouble(tmpDataStore.replayNumber);
+  m_matchDataSender->matchType.SetDouble(
+      static_cast<int>(tmpDataStore.matchType));
+
+  HAL_ControlWord ctlWord;
+  {
+    // Valid, as in other places we guarentee ctlWord >= int32
+    std::lock_guard<wpi::mutex> lock(m_controlWordMutex);
+    ctlWord = m_controlWordCache;
+  }
+  int32_t wordInt = 0;
+  std::memcpy(&wordInt, &ctlWord, sizeof(wordInt));
+  m_matchDataSender->controlWord.SetDouble(wordInt);
+}
+
 /**
  * Copy data from the DS task for the user.
  *
@@ -670,30 +769,37 @@ void DriverStation::GetData() {
   // Force a control word update, to make sure the data is the newest.
   HAL_ControlWord controlWord;
   UpdateControlWord(true, controlWord);
-  // Obtain a write lock on the data, swap the cached data into the
-  // main data arrays
-  std::lock_guard<wpi::mutex> lock(m_cacheDataMutex);
 
-  for (int32_t i = 0; i < kJoystickPorts; i++) {
-    // If buttons weren't pressed and are now, set flags in m_buttonsPressed
-    m_joystickButtonsPressed[i] |=
-        ~m_joystickButtons[i].buttons & m_joystickButtonsCache[i].buttons;
+  {
+    // Obtain a write lock on the data, swap the cached data into the
+    // main data arrays
+    std::lock_guard<wpi::mutex> lock(m_cacheDataMutex);
 
-    // If buttons were pressed and aren't now, set flags in m_buttonsReleased
-    m_joystickButtonsReleased[i] |=
-        m_joystickButtons[i].buttons & ~m_joystickButtonsCache[i].buttons;
+    for (int32_t i = 0; i < kJoystickPorts; i++) {
+      // If buttons weren't pressed and are now, set flags in m_buttonsPressed
+      m_joystickButtonsPressed[i] |=
+          ~m_joystickButtons[i].buttons & m_joystickButtonsCache[i].buttons;
+
+      // If buttons were pressed and aren't now, set flags in m_buttonsReleased
+      m_joystickButtonsReleased[i] |=
+          m_joystickButtons[i].buttons & ~m_joystickButtonsCache[i].buttons;
+    }
+
+    m_joystickAxes.swap(m_joystickAxesCache);
+    m_joystickPOVs.swap(m_joystickPOVsCache);
+    m_joystickButtons.swap(m_joystickButtonsCache);
+    m_joystickDescriptor.swap(m_joystickDescriptorCache);
+    m_matchInfo.swap(m_matchInfoCache);
   }
 
-  m_joystickAxes.swap(m_joystickAxesCache);
-  m_joystickPOVs.swap(m_joystickPOVsCache);
-  m_joystickButtons.swap(m_joystickButtonsCache);
-  m_joystickDescriptor.swap(m_joystickDescriptorCache);
-  m_matchInfo.swap(m_matchInfoCache);
+  {
+    std::lock_guard<wpi::mutex> waitLock(m_waitForDataMutex);
+    // Nofify all threads
+    m_waitForDataCounter++;
+    m_waitForDataCond.notify_all();
+  }
 
-  std::lock_guard<wpi::mutex> waitLock(m_waitForDataMutex);
-  // Nofify all threads
-  m_waitForDataCounter++;
-  m_waitForDataCond.notify_all();
+  SendMatchData();
 }
 
 /**
@@ -717,6 +823,8 @@ DriverStation::DriverStation() {
       std::make_unique<HAL_JoystickDescriptor[]>(kJoystickPorts);
   m_matchInfoCache = std::make_unique<MatchInfoData>();
 
+  m_matchDataSender = std::make_unique<MatchDataSender>();
+
   // All joysticks should default to having zero axes, povs and buttons, so
   // uninitialized memory doesn't get sent to speed controllers.
   for (unsigned int i = 0; i < kJoystickPorts; i++) {

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

@@ -25,6 +25,7 @@
 namespace frc {
 
 struct MatchInfoData;
+class MatchDataSender;
 
 /**
  * Provide access to the network communication data to / from the Driver
@@ -133,6 +134,7 @@ class DriverStation : public ErrorBase, public RobotStateInterface {
   void ReportJoystickUnpluggedWarning(const llvm::Twine& message);
   void Run();
   void UpdateControlWord(bool force, HAL_ControlWord& controlWord) const;
+  void SendMatchData();
 
   // Joystick User Data
   std::unique_ptr<HAL_JoystickAxes[]> m_joystickAxes;
@@ -148,6 +150,8 @@ class DriverStation : public ErrorBase, public RobotStateInterface {
   std::unique_ptr<HAL_JoystickDescriptor[]> m_joystickDescriptorCache;
   std::unique_ptr<MatchInfoData> m_matchInfoCache;
 
+  std::unique_ptr<MatchDataSender> m_matchDataSender;
+
   // Joystick button rising/falling edge flags
   std::array<uint32_t, kJoystickPorts> m_joystickButtonsPressed;
   std::array<uint32_t, kJoystickPorts> m_joystickButtonsReleased;

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

@@ -13,6 +13,9 @@ import java.util.concurrent.locks.Condition;
 import java.util.concurrent.locks.Lock;
 import java.util.concurrent.locks.ReentrantLock;
 
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.hal.AllianceStationID;
 import edu.wpi.first.wpilibj.hal.ControlWord;
 import edu.wpi.first.wpilibj.hal.HAL;
@@ -77,6 +80,51 @@ public class DriverStation implements RobotState.Interface {
     }
   } /* DriverStationTask */
 
+  private static class MatchDataSender {
+    @SuppressWarnings("MemberName")
+    NetworkTable table;
+    @SuppressWarnings("MemberName")
+    NetworkTableEntry typeMetadata;
+    @SuppressWarnings("MemberName")
+    NetworkTableEntry gameSpecificMessage;
+    @SuppressWarnings("MemberName")
+    NetworkTableEntry eventName;
+    @SuppressWarnings("MemberName")
+    NetworkTableEntry matchNumber;
+    @SuppressWarnings("MemberName")
+    NetworkTableEntry replayNumber;
+    @SuppressWarnings("MemberName")
+    NetworkTableEntry matchType;
+    @SuppressWarnings("MemberName")
+    NetworkTableEntry alliance;
+    @SuppressWarnings("MemberName")
+    NetworkTableEntry station;
+    @SuppressWarnings("MemberName")
+    NetworkTableEntry controlWord;
+
+    MatchDataSender() {
+      table = NetworkTableInstance.getDefault().getTable("FMSInfo");
+      typeMetadata = table.getEntry(".type");
+      typeMetadata.forceSetString("FMSInfo");
+      gameSpecificMessage = table.getEntry("GameSpecificMessage");
+      gameSpecificMessage.forceSetString("");
+      eventName = table.getEntry("EventName");
+      eventName.forceSetString("");
+      matchNumber = table.getEntry("MatchNumber");
+      matchNumber.forceSetDouble(0);
+      replayNumber = table.getEntry("ReplayNumber");
+      replayNumber.forceSetDouble(0);
+      matchType = table.getEntry("MatchType");
+      matchType.forceSetDouble(0);
+      alliance = table.getEntry("IsRedAlliance");
+      alliance.forceSetBoolean(true);
+      station = table.getEntry("StationNumber");
+      station.forceSetDouble(1);
+      controlWord = table.getEntry("FMSControlData");
+      controlWord.forceSetDouble(0);
+    }
+  }
+
   private static DriverStation instance = new DriverStation();
 
   // Joystick User Data
@@ -98,6 +146,8 @@ public class DriverStation implements RobotState.Interface {
   // preallocated byte buffer for button count
   private ByteBuffer m_buttonCountBuffer = ByteBuffer.allocateDirect(1);
 
+  private MatchDataSender m_matchDataSender;
+
   // Internal Driver Station thread
   private Thread m_thread;
   private volatile boolean m_threadKeepAlive = true;
@@ -157,6 +207,8 @@ public class DriverStation implements RobotState.Interface {
     m_controlWordCache = new ControlWord();
     m_lastControlWordUpdate = 0;
 
+    m_matchDataSender = new MatchDataSender();
+
     m_thread = new Thread(new DriverStationTask(this), "FRCDriverStation");
     m_thread.setPriority((Thread.NORM_PRIORITY + Thread.MAX_PRIORITY) / 2);
 
@@ -908,6 +960,61 @@ public class DriverStation implements RobotState.Interface {
     m_userInTest = entering;
   }
 
+  private void sendMatchData() {
+    AllianceStationID alliance = HAL.getAllianceStation();
+    boolean isRedAlliance = false;
+    int stationNumber = 1;
+    switch (alliance) {
+      case Blue1:
+        isRedAlliance = false;
+        stationNumber = 1;
+        break;
+      case Blue2:
+        isRedAlliance = false;
+        stationNumber = 2;
+        break;
+      case Blue3:
+        isRedAlliance = false;
+        stationNumber = 3;
+        break;
+      case Red1:
+        isRedAlliance = true;
+        stationNumber = 1;
+        break;
+      case Red2:
+        isRedAlliance = true;
+        stationNumber = 2;
+        break;
+      default:
+        isRedAlliance = true;
+        stationNumber = 3;
+        break;
+    }
+
+
+    String eventName;
+    String gameSpecificMessage;
+    int matchNumber;
+    int replayNumber;
+    int matchType;
+    synchronized (m_cacheDataMutex) {
+      eventName = m_matchInfo.eventName;
+      gameSpecificMessage = m_matchInfo.gameSpecificMessage;
+      matchNumber = m_matchInfo.matchNumber;
+      replayNumber = m_matchInfo.replayNumber;
+      matchType = m_matchInfo.matchType;
+    }
+
+    m_matchDataSender.alliance.setBoolean(isRedAlliance);
+    m_matchDataSender.station.setDouble(stationNumber);
+    m_matchDataSender.eventName.setString(eventName);
+    m_matchDataSender.gameSpecificMessage.setString(gameSpecificMessage);
+    m_matchDataSender.matchNumber.setDouble(matchNumber);
+    m_matchDataSender.replayNumber.setDouble(replayNumber);
+    m_matchDataSender.matchType.setDouble(matchType);
+    m_matchDataSender.controlWord.setDouble(HAL.nativeGetControlWord());
+  }
+
   /**
    * Copy data from the DS task for the user. If no new data exists, it will just be returned,
    * otherwise the data will be copied from the DS polling loop.
@@ -962,6 +1069,8 @@ public class DriverStation implements RobotState.Interface {
     m_waitForDataCount++;
     m_waitForDataCond.signalAll();
     m_waitForDataMutex.unlock();
+
+    sendMatchData();
   }
 
   /**

wpilibj/src/main/java/edu/wpi/first/wpilibj/hal/HAL.java

@@ -52,7 +52,7 @@ public class HAL extends JNIWrapper {
    */
   public static native int report(int resource, int instanceNumber, int context, String feature);
 
-  private static native int nativeGetControlWord();
+  public static native int nativeGetControlWord();
 
   @SuppressWarnings("JavadocMethod")
   public static void getControlWord(ControlWord controlWord) {