SCRIPT Move cc files

wpilibc/src/main/native/cpp/system/DataLogManager.cpp

@@ -0,0 +1,394 @@
+// 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 "frc/DataLogManager.h"
+
+#include <frc/Errors.h>
+
+#include <algorithm>
+#include <ctime>
+#include <random>
+#include <string>
+#include <vector>
+
+#include <fmt/chrono.h>
+#include <hal/UsageReporting.h>
+#include <networktables/NetworkTableInstance.h>
+#include <wpi/SafeThread.h>
+#include <wpi/StringExtras.h>
+#include <wpi/datalog/DataLog.h>
+#include <wpi/datalog/DataLogBackgroundWriter.h>
+#include <wpi/datalog/FileLogger.h>
+#include <wpi/fs.h>
+#include <wpi/print.h>
+#include <wpi/timestamp.h>
+
+#include "frc/DriverStation.h"
+#include "frc/Filesystem.h"
+#include "frc/RobotBase.h"
+#include "frc/RobotController.h"
+
+using namespace frc;
+
+namespace {
+
+struct Thread final : public wpi::SafeThread {
+  Thread(std::string_view dir, std::string_view filename, double period);
+  ~Thread() override;
+
+  void Main() final;
+
+  void StartNTLog();
+  void StopNTLog();
+  void StartConsoleLog();
+  void StopConsoleLog();
+
+  std::string m_logDir;
+  bool m_filenameOverride;
+  wpi::log::DataLogBackgroundWriter m_log;
+  bool m_ntLoggerEnabled = false;
+  NT_DataLogger m_ntEntryLogger = 0;
+  NT_ConnectionDataLogger m_ntConnLogger = 0;
+  bool m_consoleLoggerEnabled = false;
+  wpi::log::FileLogger m_consoleLogger;
+  wpi::log::StringLogEntry m_messageLog;
+};
+
+struct Instance {
+  Instance(std::string_view dir, std::string_view filename, double period);
+  wpi::SafeThreadOwner<Thread> owner;
+};
+
+}  // namespace
+
+// if less than this much free space, delete log files until there is this much
+// free space OR there are this many files remaining.
+static constexpr uintmax_t kFreeSpaceThreshold = 50000000;
+static constexpr int kFileCountThreshold = 10;
+
+static std::string MakeLogDir(std::string_view dir) {
+  if (!dir.empty()) {
+    return std::string{dir};
+  }
+#ifdef __FRC_SYSTEMCORE__
+  // prefer a mounted USB drive if one is accessible
+  std::error_code ec;
+  auto s = fs::status("/u", ec);
+  if (!ec && fs::is_directory(s) &&
+      (s.permissions() & fs::perms::others_write) != fs::perms::none) {
+    fs::create_directory("/u/logs", ec);
+    return "/u/logs";
+    HAL_ReportUsage("DataLogManager", "USB");
+  }
+  fs::create_directory("/home/systemcore/logs", ec);
+  HAL_ReportUsage("DataLogManager", "Onboard");
+  return "/home/systemcore/logs";
+#else
+  std::string logDir = filesystem::GetOperatingDirectory() + "/logs";
+  std::error_code ec;
+  fs::create_directory(logDir, ec);
+  return logDir;
+#endif
+}
+
+static std::string MakeLogFilename(std::string_view filenameOverride) {
+  if (!filenameOverride.empty()) {
+    return std::string{filenameOverride};
+  }
+  static std::random_device dev;
+  static std::mt19937 rng(dev());
+  std::uniform_int_distribution<int> dist(0, 15);
+  const char* v = "0123456789abcdef";
+  std::string filename = "FRC_TBD_";
+  for (int i = 0; i < 16; i++) {
+    filename += v[dist(rng)];
+  }
+  filename += ".wpilog";
+  return filename;
+}
+
+Thread::Thread(std::string_view dir, std::string_view filename, double period)
+    : m_logDir{dir},
+      m_filenameOverride{!filename.empty()},
+      m_log{dir, MakeLogFilename(filename), period},
+      m_messageLog{m_log, "messages"} {
+  StartNTLog();
+  StartConsoleLog();
+}
+
+Thread::~Thread() {
+  StopNTLog();
+  StopConsoleLog();
+}
+
+void Thread::Main() {
+  // based on free disk space, scan for "old" FRC_*.wpilog files and remove
+  {
+    std::error_code ec;
+    uintmax_t freeSpace;
+    auto freeSpaceInfo = fs::space(m_logDir, ec);
+    if (!ec) {
+      freeSpace = freeSpaceInfo.available;
+    } else {
+      freeSpace = UINTMAX_MAX;
+    }
+    if (freeSpace < kFreeSpaceThreshold) {
+      // Delete oldest FRC_*.wpilog files (ignore FRC_TBD_*.wpilog as we just
+      // created one)
+      std::vector<fs::directory_entry> entries;
+      for (auto&& entry : fs::directory_iterator{m_logDir, ec}) {
+        auto stem = entry.path().stem().string();
+        if (wpi::starts_with(stem, "FRC_") &&
+            entry.path().extension() == ".wpilog" &&
+            !wpi::starts_with(stem, "FRC_TBD_")) {
+          entries.emplace_back(entry);
+        }
+      }
+      std::sort(entries.begin(), entries.end(),
+                [](const auto& a, const auto& b) {
+                  return a.last_write_time() < b.last_write_time();
+                });
+
+      int count = entries.size();
+      for (auto&& entry : entries) {
+        --count;
+        if (count < kFileCountThreshold) {
+          break;
+        }
+        auto size = entry.file_size();
+        if (fs::remove(entry.path(), ec)) {
+          FRC_ReportWarning("DataLogManager: Deleted {}",
+                            entry.path().string());
+          freeSpace += size;
+          if (freeSpace >= kFreeSpaceThreshold) {
+            break;
+          }
+        } else {
+          wpi::print(stderr, "DataLogManager: could not delete {}\n",
+                     entry.path().string());
+        }
+      }
+    } else if (freeSpace < 2 * kFreeSpaceThreshold) {
+      FRC_ReportError(
+          warn::Warning,
+          "DataLogManager: Log storage device has {} MB of free space "
+          "remaining! Logs will get deleted below {} MB of free space. "
+          "Consider deleting logs off the storage device.",
+          freeSpace / 1000000, kFreeSpaceThreshold / 1000000);
+    }
+  }
+
+  int timeoutCount = 0;
+  bool paused = false;
+  int dsAttachCount = 0;
+  int fmsAttachCount = 0;
+  bool dsRenamed = m_filenameOverride;
+  bool fmsRenamed = m_filenameOverride;
+  int sysTimeCount = 0;
+  wpi::log::IntegerLogEntry sysTimeEntry{
+      m_log, "systemTime",
+      "{\"source\":\"DataLogManager\",\"format\":\"time_t_us\"}"};
+
+  wpi::Event newDataEvent;
+  DriverStation::ProvideRefreshedDataEventHandle(newDataEvent.GetHandle());
+
+  for (;;) {
+    bool timedOut = false;
+    bool newData =
+        wpi::WaitForObject(newDataEvent.GetHandle(), 0.25, &timedOut);
+    if (!m_active) {
+      break;
+    }
+    if (!newData) {
+      ++timeoutCount;
+      // pause logging after being disconnected for 10 seconds
+      if (timeoutCount > 40 && !paused) {
+        timeoutCount = 0;
+        paused = true;
+        m_log.Pause();
+      }
+      continue;
+    }
+    // when we connect to the DS, resume logging
+    timeoutCount = 0;
+    if (paused) {
+      paused = false;
+      m_log.Resume();
+    }
+
+    if (!dsRenamed) {
+      // track DS attach
+      if (DriverStation::IsDSAttached()) {
+        ++dsAttachCount;
+      } else {
+        dsAttachCount = 0;
+      }
+      if (dsAttachCount > 50) {  // 1 second
+        if (RobotController::IsSystemTimeValid()) {
+          std::time_t now = std::time(nullptr);
+          auto tm = std::gmtime(&now);
+          m_log.SetFilename(fmt::format("FRC_{:%Y%m%d_%H%M%S}.wpilog", *tm));
+          dsRenamed = true;
+        } else {
+          dsAttachCount = 0;  // wait a bit and try again
+        }
+      }
+    }
+
+    if (!fmsRenamed) {
+      // track FMS attach
+      if (DriverStation::IsFMSAttached()) {
+        ++fmsAttachCount;
+      } else {
+        fmsAttachCount = 0;
+      }
+      if (fmsAttachCount > 250) {  // 5 seconds
+        // match info comes through TCP, so we need to double-check we've
+        // actually received it
+        auto matchType = DriverStation::GetMatchType();
+        if (matchType != DriverStation::kNone) {
+          // rename per match info
+          char matchTypeChar;
+          switch (matchType) {
+            case DriverStation::kPractice:
+              matchTypeChar = 'P';
+              break;
+            case DriverStation::kQualification:
+              matchTypeChar = 'Q';
+              break;
+            case DriverStation::kElimination:
+              matchTypeChar = 'E';
+              break;
+            default:
+              matchTypeChar = '_';
+              break;
+          }
+          std::time_t now = std::time(nullptr);
+          m_log.SetFilename(
+              fmt::format("FRC_{:%Y%m%d_%H%M%S}_{}_{}{}.wpilog",
+                          *std::gmtime(&now), DriverStation::GetEventName(),
+                          matchTypeChar, DriverStation::GetMatchNumber()));
+          fmsRenamed = true;
+          dsRenamed = true;  // don't override FMS rename
+        }
+      }
+    }
+
+    // Write system time every ~5 seconds
+    ++sysTimeCount;
+    if (sysTimeCount >= 250) {
+      sysTimeCount = 0;
+      if (RobotController::IsSystemTimeValid()) {
+        sysTimeEntry.Append(wpi::GetSystemTime(), wpi::Now());
+      }
+    }
+  }
+  DriverStation::RemoveRefreshedDataEventHandle(newDataEvent.GetHandle());
+}
+
+void Thread::StartNTLog() {
+  if (!m_ntLoggerEnabled) {
+    m_ntLoggerEnabled = true;
+    auto inst = nt::NetworkTableInstance::GetDefault();
+    m_ntEntryLogger = inst.StartEntryDataLog(m_log, "", "NT:");
+    m_ntConnLogger = inst.StartConnectionDataLog(m_log, "NTConnection");
+  }
+}
+
+void Thread::StopNTLog() {
+  if (m_ntLoggerEnabled) {
+    m_ntLoggerEnabled = false;
+    nt::NetworkTableInstance::StopEntryDataLog(m_ntEntryLogger);
+    nt::NetworkTableInstance::StopConnectionDataLog(m_ntConnLogger);
+  }
+}
+
+void Thread::StartConsoleLog() {
+  if (!m_consoleLoggerEnabled && RobotBase::IsReal()) {
+    m_consoleLoggerEnabled = true;
+    m_consoleLogger = {"/home/systemcore/FRC_UserProgram.log", m_log,
+                       "console"};
+  }
+}
+
+void Thread::StopConsoleLog() {
+  if (m_consoleLoggerEnabled && RobotBase::IsReal()) {
+    m_consoleLoggerEnabled = false;
+    m_consoleLogger = {};
+  }
+}
+
+Instance::Instance(std::string_view dir, std::string_view filename,
+                   double period) {
+  // Delete all previously existing FRC_TBD_*.wpilog files. These only exist
+  // when the robot never connects to the DS, so they are very unlikely to
+  // have useful data and just clutter the filesystem.
+  auto logDir = MakeLogDir(dir);
+  std::error_code ec;
+  for (auto&& entry : fs::directory_iterator{logDir, ec}) {
+    if (wpi::starts_with(entry.path().stem().string(), "FRC_TBD_") &&
+        entry.path().extension() == ".wpilog") {
+      if (!fs::remove(entry, ec)) {
+        wpi::print(stderr, "DataLogManager: could not delete {}\n",
+                   entry.path().string());
+      }
+    }
+  }
+
+  owner.Start(logDir, filename, period);
+}
+
+static Instance& GetInstance(std::string_view dir = "",
+                             std::string_view filename = "",
+                             double period = 0.25) {
+  static Instance instance(dir, filename, period);
+  if (!instance.owner) {
+    instance.owner.Start(MakeLogDir(dir), filename, period);
+  }
+  return instance;
+}
+
+void DataLogManager::Start(std::string_view dir, std::string_view filename,
+                           double period) {
+  GetInstance(dir, filename, period);
+}
+
+void DataLogManager::Stop() {
+  auto& inst = GetInstance();
+  inst.owner.GetThread()->m_log.Stop();
+  inst.owner.Join();
+}
+
+void DataLogManager::Log(std::string_view message) {
+  GetInstance().owner.GetThread()->m_messageLog.Append(message);
+  wpi::print("{}\n", message);
+}
+
+wpi::log::DataLog& DataLogManager::GetLog() {
+  return GetInstance().owner.GetThread()->m_log;
+}
+
+std::string DataLogManager::GetLogDir() {
+  return GetInstance().owner.GetThread()->m_logDir;
+}
+
+void DataLogManager::LogNetworkTables(bool enabled) {
+  if (auto thr = GetInstance().owner.GetThread()) {
+    if (enabled) {
+      thr->StartNTLog();
+    } else if (!enabled) {
+      thr->StopNTLog();
+    }
+  }
+}
+
+void DataLogManager::LogConsoleOutput(bool enabled) {
+  if (auto thr = GetInstance().owner.GetThread()) {
+    if (enabled) {
+      thr->StartConsoleLog();
+    } else if (!enabled) {
+      thr->StopConsoleLog();
+    }
+  }
+}

wpilibc/src/main/native/cpp/system/Errors.cpp

@@ -0,0 +1,82 @@
+// 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 "frc/Errors.h"
+
+#include <string>
+#include <utility>
+
+#include <hal/DriverStation.h>
+#include <hal/HALBase.h>
+#include <wpi/StackTrace.h>
+#include <wpi/fs.h>
+
+using namespace frc;
+
+RuntimeError::RuntimeError(int32_t code, std::string&& loc, std::string&& stack,
+                           std::string&& message)
+    : runtime_error{std::move(message)}, m_data{std::make_shared<Data>()} {
+  m_data->code = code;
+  m_data->loc = std::move(loc);
+  m_data->stack = stack;
+}
+
+RuntimeError::RuntimeError(int32_t code, const char* fileName, int lineNumber,
+                           const char* funcName, std::string&& stack,
+                           std::string&& message)
+    : RuntimeError{
+          code,
+          fmt::format("{} [{}:{}]", funcName,
+                      fs::path{fileName}.filename().string(), lineNumber),
+          std::move(stack), std::move(message)} {}
+
+void RuntimeError::Report() const {
+  HAL_SendError(m_data->code < 0, m_data->code, 0, what(), m_data->loc.c_str(),
+                m_data->stack.c_str(), 1);
+}
+
+const char* frc::GetErrorMessage(int32_t* code) {
+  switch (*code) {
+#define S(label, offset, message) \
+  case err::label:                \
+    return message;
+#include "frc/WPIErrors.mac"
+#undef S
+#define S(label, offset, message) \
+  case warn::label:               \
+    return message;
+#include "frc/WPIWarnings.mac"
+#undef S
+    default:
+      return HAL_GetLastError(code);
+  }
+}
+
+void frc::ReportErrorV(int32_t status, const char* fileName, int lineNumber,
+                       const char* funcName, fmt::string_view format,
+                       fmt::format_args args) {
+  if (status == 0) {
+    return;
+  }
+  fmt::memory_buffer out;
+  fmt::format_to(fmt::appender{out}, "{}: ", GetErrorMessage(&status));
+  fmt::vformat_to(fmt::appender{out}, format, args);
+  out.push_back('\0');
+  HAL_SendError(status < 0, status, 0, out.data(), funcName,
+                wpi::GetStackTrace(2).c_str(), 1);
+}
+
+RuntimeError frc::MakeErrorV(int32_t status, const char* fileName,
+                             int lineNumber, const char* funcName,
+                             fmt::string_view format, fmt::format_args args) {
+  fmt::memory_buffer out;
+  fmt::format_to(fmt::appender{out}, "{}: ", GetErrorMessage(&status));
+  fmt::vformat_to(fmt::appender{out}, format, args);
+  return RuntimeError{status,
+                      fileName,
+                      lineNumber,
+                      funcName,
+                      wpi::GetStackTrace(2),
+                      fmt::to_string(out)};
+}

wpilibc/src/main/native/cpp/system/Filesystem.cpp

@@ -0,0 +1,31 @@
+// 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 "frc/Filesystem.h"
+
+#include <string>
+
+#include <wpi/fs.h>
+
+#include "frc/RobotBase.h"
+
+std::string frc::filesystem::GetLaunchDirectory() {
+  return fs::current_path().string();
+}
+
+std::string frc::filesystem::GetOperatingDirectory() {
+  if constexpr (!RobotBase::IsSimulation()) {
+    return "/home/systemcore";
+  } else {
+    return frc::filesystem::GetLaunchDirectory();
+  }
+}
+
+std::string frc::filesystem::GetDeployDirectory() {
+  if constexpr (!RobotBase::IsSimulation()) {
+    return "/home/systemcore/deploy";
+  } else {
+    return (fs::current_path() / "src" / "main" / "deploy").string();
+  }
+}

wpilibc/src/main/native/cpp/system/Notifier.cpp

@@ -0,0 +1,214 @@
+// 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 "frc/Notifier.h"
+
+#include <utility>
+
+#include <fmt/format.h>
+#include <hal/DriverStation.h>
+#include <hal/Notifier.h>
+#include <hal/Threads.h>
+
+#include "frc/Errors.h"
+#include "frc/Timer.h"
+
+using namespace frc;
+
+Notifier::Notifier(std::function<void()> callback) {
+  if (!callback) {
+    throw FRC_MakeError(err::NullParameter, "callback");
+  }
+  m_callback = callback;
+  int32_t status = 0;
+  m_notifier = HAL_InitializeNotifier(&status);
+  FRC_CheckErrorStatus(status, "InitializeNotifier");
+
+  m_thread = std::thread([=, this] {
+    for (;;) {
+      int32_t status = 0;
+      HAL_NotifierHandle notifier = m_notifier.load();
+      if (notifier == 0) {
+        break;
+      }
+      uint64_t curTime = HAL_WaitForNotifierAlarm(notifier, &status);
+      if (curTime == 0 || status != 0) {
+        break;
+      }
+
+      std::function<void()> callback;
+      {
+        std::scoped_lock lock(m_processMutex);
+        callback = m_callback;
+        if (m_periodic) {
+          m_expirationTime += m_period;
+          UpdateAlarm();
+        } else {
+          // Need to update the alarm to cause it to wait again
+          UpdateAlarm(UINT64_MAX);
+        }
+      }
+
+      // Call callback
+      if (callback) {
+        callback();
+      }
+    }
+  });
+}
+
+Notifier::Notifier(int priority, std::function<void()> callback) {
+  if (!callback) {
+    throw FRC_MakeError(err::NullParameter, "callback");
+  }
+  m_callback = callback;
+  int32_t status = 0;
+  m_notifier = HAL_InitializeNotifier(&status);
+  FRC_CheckErrorStatus(status, "InitializeNotifier");
+
+  m_thread = std::thread([=, this] {
+    int32_t status = 0;
+    HAL_SetCurrentThreadPriority(true, priority, &status);
+    for (;;) {
+      HAL_NotifierHandle notifier = m_notifier.load();
+      if (notifier == 0) {
+        break;
+      }
+      uint64_t curTime = HAL_WaitForNotifierAlarm(notifier, &status);
+      if (curTime == 0 || status != 0) {
+        break;
+      }
+
+      std::function<void()> callback;
+      {
+        std::scoped_lock lock(m_processMutex);
+        callback = m_callback;
+        if (m_periodic) {
+          m_expirationTime += m_period;
+          UpdateAlarm();
+        } else {
+          // need to update the alarm to cause it to wait again
+          UpdateAlarm(UINT64_MAX);
+        }
+      }
+
+      // call callback
+      if (callback) {
+        try {
+          callback();
+        } catch (const frc::RuntimeError& e) {
+          e.Report();
+          FRC_ReportError(
+              err::Error,
+              "Error in Notifier thread."
+              "  The above stacktrace can help determine where the error "
+              "occurred.\n"
+              "  See https://wpilib.org/stacktrace for more information.\n");
+          throw;
+        } catch (const std::exception& e) {
+          HAL_SendError(1, err::Error, 0, e.what(), "", "", 1);
+          throw;
+        }
+      }
+    }
+  });
+}
+
+Notifier::~Notifier() {
+  int32_t status = 0;
+  // atomically set handle to 0, then clean
+  HAL_NotifierHandle handle = m_notifier.exchange(0);
+  HAL_StopNotifier(handle, &status);
+  FRC_ReportError(status, "StopNotifier");
+
+  // Join the thread to ensure the callback has exited.
+  if (m_thread.joinable()) {
+    m_thread.join();
+  }
+
+  HAL_CleanNotifier(handle);
+}
+
+Notifier::Notifier(Notifier&& rhs)
+    : m_thread(std::move(rhs.m_thread)),
+      m_notifier(rhs.m_notifier.load()),
+      m_callback(std::move(rhs.m_callback)),
+      m_expirationTime(std::move(rhs.m_expirationTime)),
+      m_period(std::move(rhs.m_period)),
+      m_periodic(std::move(rhs.m_periodic)) {
+  rhs.m_notifier = HAL_kInvalidHandle;
+}
+
+Notifier& Notifier::operator=(Notifier&& rhs) {
+  m_thread = std::move(rhs.m_thread);
+  m_notifier = rhs.m_notifier.load();
+  rhs.m_notifier = HAL_kInvalidHandle;
+  m_callback = std::move(rhs.m_callback);
+  m_expirationTime = std::move(rhs.m_expirationTime);
+  m_period = std::move(rhs.m_period);
+  m_periodic = std::move(rhs.m_periodic);
+
+  return *this;
+}
+
+void Notifier::SetName(std::string_view name) {
+  fmt::memory_buffer buf;
+  fmt::format_to(fmt::appender{buf}, "{}", name);
+  buf.push_back('\0');  // null terminate
+  int32_t status = 0;
+  HAL_SetNotifierName(m_notifier, buf.data(), &status);
+}
+
+void Notifier::SetCallback(std::function<void()> callback) {
+  std::scoped_lock lock(m_processMutex);
+  m_callback = callback;
+}
+
+void Notifier::StartSingle(units::second_t delay) {
+  std::scoped_lock lock(m_processMutex);
+  m_periodic = false;
+  m_period = delay;
+  m_expirationTime = Timer::GetFPGATimestamp() + m_period;
+  UpdateAlarm();
+}
+
+void Notifier::StartPeriodic(units::second_t period) {
+  std::scoped_lock lock(m_processMutex);
+  m_periodic = true;
+  m_period = period;
+  m_expirationTime = Timer::GetFPGATimestamp() + m_period;
+  UpdateAlarm();
+}
+
+void Notifier::StartPeriodic(units::hertz_t frequency) {
+  StartPeriodic(1 / frequency);
+}
+
+void Notifier::Stop() {
+  std::scoped_lock lock(m_processMutex);
+  m_periodic = false;
+  int32_t status = 0;
+  HAL_CancelNotifierAlarm(m_notifier, &status);
+  FRC_CheckErrorStatus(status, "CancelNotifierAlarm");
+}
+
+void Notifier::UpdateAlarm(uint64_t triggerTime) {
+  int32_t status = 0;
+  // Return if we are being destructed, or were not created successfully
+  auto notifier = m_notifier.load();
+  if (notifier == 0) {
+    return;
+  }
+  HAL_UpdateNotifierAlarm(notifier, triggerTime, &status);
+  FRC_CheckErrorStatus(status, "UpdateNotifierAlarm");
+}
+
+void Notifier::UpdateAlarm() {
+  UpdateAlarm(static_cast<uint64_t>(m_expirationTime * 1e6));
+}
+
+bool Notifier::SetHALThreadPriority(bool realTime, int32_t priority) {
+  int32_t status = 0;
+  return HAL_SetNotifierThreadPriority(realTime, priority, &status);
+}

wpilibc/src/main/native/cpp/system/Resource.cpp

@@ -0,0 +1,69 @@
+// 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 "frc/Resource.h"
+
+#include <limits>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <wpi/deprecated.h>
+
+#include "frc/Errors.h"
+
+WPI_IGNORE_DEPRECATED
+using namespace frc;
+
+wpi::mutex Resource::m_createMutex;
+
+void Resource::CreateResourceObject(std::unique_ptr<Resource>& r,
+                                    uint32_t elements) {
+  std::scoped_lock lock(m_createMutex);
+  if (!r) {
+    r = std::make_unique<Resource>(elements);
+  }
+}
+WPI_UNIGNORE_DEPRECATED
+
+Resource::Resource(uint32_t elements) {
+  m_isAllocated = std::vector<bool>(elements, false);
+}
+
+uint32_t Resource::Allocate(const std::string& resourceDesc) {
+  std::scoped_lock lock(m_allocateMutex);
+  for (uint32_t i = 0; i < m_isAllocated.size(); i++) {
+    if (!m_isAllocated[i]) {
+      m_isAllocated[i] = true;
+      return i;
+    }
+  }
+  throw FRC_MakeError(err::NoAvailableResources, "{}", resourceDesc);
+}
+
+uint32_t Resource::Allocate(uint32_t index, const std::string& resourceDesc) {
+  std::scoped_lock lock(m_allocateMutex);
+  if (index >= m_isAllocated.size()) {
+    throw FRC_MakeError(err::ChannelIndexOutOfRange, "{}", resourceDesc);
+  }
+  if (m_isAllocated[index]) {
+    throw FRC_MakeError(err::ResourceAlreadyAllocated, "{}", resourceDesc);
+  }
+  m_isAllocated[index] = true;
+  return index;
+}
+
+void Resource::Free(uint32_t index) {
+  std::unique_lock lock(m_allocateMutex);
+  if (index == std::numeric_limits<uint32_t>::max()) {
+    return;
+  }
+  if (index >= m_isAllocated.size()) {
+    throw FRC_MakeError(err::NotAllocated, "index {}", index);
+  }
+  if (!m_isAllocated[index]) {
+    throw FRC_MakeError(err::NotAllocated, "index {}", index);
+  }
+  m_isAllocated[index] = false;
+}

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

@@ -0,0 +1,180 @@
+// 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 "frc/RobotController.h"
+
+#include <functional>
+#include <string>
+
+#include <hal/CAN.h>
+#include <hal/HALBase.h>
+#include <hal/Power.h>
+
+#include "frc/Errors.h"
+
+using namespace frc;
+
+std::function<uint64_t()> RobotController::m_timeSource = [] {
+  return RobotController::GetFPGATime();
+};
+
+std::string RobotController::GetSerialNumber() {
+  WPI_String serialNum;
+  HAL_GetSerialNumber(&serialNum);
+  std::string ret{wpi::to_string_view(&serialNum)};
+  WPI_FreeString(&serialNum);
+  return ret;
+}
+
+std::string RobotController::GetComments() {
+  WPI_String comments;
+  HAL_GetComments(&comments);
+  std::string ret{wpi::to_string_view(&comments)};
+  WPI_FreeString(&comments);
+  return ret;
+}
+
+int32_t RobotController::GetTeamNumber() {
+  return HAL_GetTeamNumber();
+}
+
+void RobotController::SetTimeSource(std::function<uint64_t()> supplier) {
+  m_timeSource = supplier;
+}
+
+uint64_t RobotController::GetTime() {
+  return m_timeSource();
+}
+
+uint64_t RobotController::GetFPGATime() {
+  int32_t status = 0;
+  uint64_t time = HAL_GetFPGATime(&status);
+  FRC_CheckErrorStatus(status, "GetFPGATime");
+  return time;
+}
+
+units::volt_t RobotController::GetBatteryVoltage() {
+  int32_t status = 0;
+  double retVal = HAL_GetVinVoltage(&status);
+  FRC_CheckErrorStatus(status, "GetBatteryVoltage");
+  return units::volt_t{retVal};
+}
+
+bool RobotController::IsSysActive() {
+  int32_t status = 0;
+  bool retVal = HAL_GetSystemActive(&status);
+  FRC_CheckErrorStatus(status, "IsSysActive");
+  return retVal;
+}
+
+bool RobotController::IsBrownedOut() {
+  int32_t status = 0;
+  bool retVal = HAL_GetBrownedOut(&status);
+  FRC_CheckErrorStatus(status, "IsBrownedOut");
+  return retVal;
+}
+
+int RobotController::GetCommsDisableCount() {
+  int32_t status = 0;
+  int retVal = HAL_GetCommsDisableCount(&status);
+  FRC_CheckErrorStatus(status, "GetCommsDisableCount");
+  return retVal;
+}
+
+bool RobotController::GetRSLState() {
+  int32_t status = 0;
+  bool retVal = HAL_GetRSLState(&status);
+  FRC_CheckErrorStatus(status, "GetRSLState");
+  return retVal;
+}
+
+bool RobotController::IsSystemTimeValid() {
+  int32_t status = 0;
+  bool retVal = HAL_GetSystemTimeValid(&status);
+  FRC_CheckErrorStatus(status, "IsSystemTimeValid");
+  return retVal;
+}
+
+double RobotController::GetInputVoltage() {
+  int32_t status = 0;
+  double retVal = HAL_GetVinVoltage(&status);
+  FRC_CheckErrorStatus(status, "GetInputVoltage");
+  return retVal;
+}
+
+double RobotController::GetVoltage3V3() {
+  int32_t status = 0;
+  double retVal = HAL_GetUserVoltage3V3(&status);
+  FRC_CheckErrorStatus(status, "GetVoltage3V3");
+  return retVal;
+}
+
+double RobotController::GetCurrent3V3() {
+  int32_t status = 0;
+  double retVal = HAL_GetUserCurrent3V3(&status);
+  FRC_CheckErrorStatus(status, "GetCurrent3V3");
+  return retVal;
+}
+
+void RobotController::SetEnabled3V3(bool enabled) {
+  int32_t status = 0;
+  HAL_SetUserRailEnabled3V3(enabled, &status);
+  FRC_CheckErrorStatus(status, "SetEnabled3V3");
+}
+
+bool RobotController::GetEnabled3V3() {
+  int32_t status = 0;
+  bool retVal = HAL_GetUserActive3V3(&status);
+  FRC_CheckErrorStatus(status, "GetEnabled3V3");
+  return retVal;
+}
+
+int RobotController::GetFaultCount3V3() {
+  int32_t status = 0;
+  int retVal = HAL_GetUserCurrentFaults3V3(&status);
+  FRC_CheckErrorStatus(status, "GetFaultCount3V3");
+  return retVal;
+}
+
+void RobotController::ResetRailFaultCounts() {
+  int32_t status = 0;
+  HAL_ResetUserCurrentFaults(&status);
+  FRC_CheckErrorStatus(status, "ResetRailFaultCounts");
+}
+
+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.value(), &status);
+  FRC_CheckErrorStatus(status, "SetBrownoutVoltage");
+}
+
+units::celsius_t RobotController::GetCPUTemp() {
+  int32_t status = 0;
+  double retVal = HAL_GetCPUTemp(&status);
+  FRC_CheckErrorStatus(status, "GetCPUTemp");
+  return units::celsius_t{retVal};
+}
+
+CANStatus RobotController::GetCANStatus(int busId) {
+  int32_t status = 0;
+  float percentBusUtilization = 0;
+  uint32_t busOffCount = 0;
+  uint32_t txFullCount = 0;
+  uint32_t receiveErrorCount = 0;
+  uint32_t transmitErrorCount = 0;
+  HAL_CAN_GetCANStatus(busId, &percentBusUtilization, &busOffCount,
+                       &txFullCount, &receiveErrorCount, &transmitErrorCount,
+                       &status);
+  FRC_CheckErrorStatus(status, "GetCANStatus");
+  return {percentBusUtilization, static_cast<int>(busOffCount),
+          static_cast<int>(txFullCount), static_cast<int>(receiveErrorCount),
+          static_cast<int>(transmitErrorCount)};
+}

wpilibc/src/main/native/cpp/system/ScopedTracer.cpp

@@ -0,0 +1,19 @@
+// 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 "frc/ScopedTracer.h"
+
+#include <wpi/raw_ostream.h>
+
+using namespace frc;
+
+ScopedTracer::ScopedTracer(std::string_view name, wpi::raw_ostream& os)
+    : m_name(name), m_os(os) {
+  m_tracer.ResetTimer();
+}
+
+ScopedTracer::~ScopedTracer() {
+  m_tracer.AddEpoch(m_name);
+  m_tracer.PrintEpochs(m_os);
+}

wpilibc/src/main/native/cpp/system/SystemServer.cpp

@@ -0,0 +1,15 @@
+// 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 "frc/SystemServer.h"
+
+#include <hal/SystemServer.h>
+
+namespace frc {
+
+nt::NetworkTableInstance SystemServer::GetSystemServer() {
+  return nt::NetworkTableInstance{HAL_GetSystemServerHandle()};
+}
+
+}  // namespace frc

wpilibc/src/main/native/cpp/system/Threads.cpp

@@ -0,0 +1,47 @@
+// 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 "frc/Threads.h"
+
+#include <hal/Threads.h>
+
+#include "frc/Errors.h"
+
+namespace frc {
+
+int GetThreadPriority(std::thread& thread, bool* isRealTime) {
+  int32_t status = 0;
+  HAL_Bool rt = false;
+  auto native = thread.native_handle();
+  auto ret = HAL_GetThreadPriority(&native, &rt, &status);
+  FRC_CheckErrorStatus(status, "GetThreadPriority");
+  *isRealTime = rt;
+  return ret;
+}
+
+int GetCurrentThreadPriority(bool* isRealTime) {
+  int32_t status = 0;
+  HAL_Bool rt = false;
+  auto ret = HAL_GetCurrentThreadPriority(&rt, &status);
+  FRC_CheckErrorStatus(status, "GetCurrentThreadPriority");
+  *isRealTime = rt;
+  return ret;
+}
+
+bool SetThreadPriority(std::thread& thread, bool realTime, int priority) {
+  int32_t status = 0;
+  auto native = thread.native_handle();
+  auto ret = HAL_SetThreadPriority(&native, realTime, priority, &status);
+  FRC_CheckErrorStatus(status, "SetThreadPriority");
+  return ret;
+}
+
+bool SetCurrentThreadPriority(bool realTime, int priority) {
+  int32_t status = 0;
+  auto ret = HAL_SetCurrentThreadPriority(realTime, priority, &status);
+  FRC_CheckErrorStatus(status, "SetCurrentThreadPriority");
+  return ret;
+}
+
+}  // namespace frc

wpilibc/src/main/native/cpp/system/Timer.cpp

@@ -0,0 +1,102 @@
+// 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 "frc/Timer.h"
+
+#include <chrono>
+#include <thread>
+
+#include "frc/DriverStation.h"
+#include "frc/RobotController.h"
+
+namespace frc {
+
+void Wait(units::second_t seconds) {
+  std::this_thread::sleep_for(std::chrono::duration<double>(seconds.value()));
+}
+
+units::second_t GetTime() {
+  using std::chrono::duration;
+  using std::chrono::duration_cast;
+  using std::chrono::system_clock;
+
+  return units::second_t{
+      duration_cast<duration<double>>(system_clock::now().time_since_epoch())
+          .count()};
+}
+
+}  // namespace frc
+
+using namespace frc;
+
+Timer::Timer() {
+  Reset();
+}
+
+units::second_t Timer::Get() const {
+  if (m_running) {
+    return (GetTimestamp() - m_startTime) + m_accumulatedTime;
+  } else {
+    return m_accumulatedTime;
+  }
+}
+
+void Timer::Reset() {
+  m_accumulatedTime = 0_s;
+  m_startTime = GetTimestamp();
+}
+
+void Timer::Start() {
+  if (!m_running) {
+    m_startTime = GetTimestamp();
+    m_running = true;
+  }
+}
+
+void Timer::Restart() {
+  if (m_running) {
+    Stop();
+  }
+  Reset();
+  Start();
+}
+
+void Timer::Stop() {
+  if (m_running) {
+    m_accumulatedTime = Get();
+    m_running = false;
+  }
+}
+
+bool Timer::HasElapsed(units::second_t period) const {
+  return Get() >= period;
+}
+
+bool Timer::AdvanceIfElapsed(units::second_t period) {
+  if (Get() >= period) {
+    // Advance the start time by the period.
+    m_startTime += period;
+    // Don't set it to the current time... we want to avoid drift.
+    return true;
+  } else {
+    return false;
+  }
+}
+
+bool Timer::IsRunning() const {
+  return m_running;
+}
+
+units::second_t Timer::GetTimestamp() {
+  return units::second_t{frc::RobotController::GetTime() * 1.0e-6};
+}
+
+units::second_t Timer::GetFPGATimestamp() {
+  // FPGA returns the timestamp in microseconds
+  return units::second_t{frc::RobotController::GetFPGATime() * 1.0e-6};
+}
+
+units::second_t Timer::GetMatchTime() {
+  return frc::DriverStation::GetMatchTime();
+}

wpilibc/src/main/native/cpp/system/Tracer.cpp

@@ -0,0 +1,56 @@
+// 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 "frc/Tracer.h"
+
+#include <fmt/format.h>
+#include <wpi/SmallString.h>
+#include <wpi/raw_ostream.h>
+
+#include "frc/Errors.h"
+
+using namespace frc;
+
+Tracer::Tracer() {
+  ResetTimer();
+}
+
+void Tracer::ResetTimer() {
+  m_startTime = hal::fpga_clock::now();
+}
+
+void Tracer::ClearEpochs() {
+  ResetTimer();
+  m_epochs.clear();
+}
+
+void Tracer::AddEpoch(std::string_view epochName) {
+  auto currentTime = hal::fpga_clock::now();
+  m_epochs[epochName] = currentTime - m_startTime;
+  m_startTime = currentTime;
+}
+
+void Tracer::PrintEpochs() {
+  wpi::SmallString<128> buf;
+  wpi::raw_svector_ostream os(buf);
+  PrintEpochs(os);
+  if (!buf.empty()) {
+    FRC_ReportWarning("{}", buf.c_str());
+  }
+}
+
+void Tracer::PrintEpochs(wpi::raw_ostream& os) {
+  using std::chrono::duration_cast;
+  using std::chrono::microseconds;
+
+  auto now = hal::fpga_clock::now();
+  if (now - m_lastEpochsPrintTime > kMinPrintPeriod) {
+    m_lastEpochsPrintTime = now;
+    for (const auto& epoch : m_epochs) {
+      os << fmt::format(
+          "\t{}: {:.6f}s\n", epoch.first,
+          duration_cast<microseconds>(epoch.second).count() / 1.0e6);
+    }
+  }
+}

wpilibc/src/main/native/cpp/system/Watchdog.cpp

@@ -0,0 +1,242 @@
+// 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 "frc/Watchdog.h"
+
+#include <atomic>
+#include <thread>
+#include <utility>
+#include <vector>
+
+#include <fmt/format.h>
+#include <hal/Notifier.h>
+#include <wpi/mutex.h>
+#include <wpi/priority_queue.h>
+
+#include "frc/Errors.h"
+#include "frc/Timer.h"
+
+using namespace frc;
+
+class Watchdog::Impl {
+ public:
+  Impl();
+  ~Impl();
+
+  template <typename T>
+  struct DerefGreater {
+    constexpr bool operator()(const T& lhs, const T& rhs) const {
+      return *lhs > *rhs;
+    }
+  };
+
+  wpi::mutex m_mutex;
+  std::atomic<HAL_NotifierHandle> m_notifier;
+  wpi::priority_queue<Watchdog*, std::vector<Watchdog*>,
+                      DerefGreater<Watchdog*>>
+      m_watchdogs;
+
+  void UpdateAlarm();
+
+ private:
+  void Main();
+
+  std::thread m_thread;
+};
+
+Watchdog::Impl::Impl() {
+  int32_t status = 0;
+  m_notifier = HAL_InitializeNotifier(&status);
+  FRC_CheckErrorStatus(status, "starting watchdog notifier");
+  HAL_SetNotifierName(m_notifier, "Watchdog", &status);
+
+  m_thread = std::thread([=, this] { Main(); });
+}
+
+Watchdog::Impl::~Impl() {
+  int32_t status = 0;
+  // atomically set handle to 0, then clean
+  HAL_NotifierHandle handle = m_notifier.exchange(0);
+  HAL_StopNotifier(handle, &status);
+  FRC_ReportError(status, "stopping watchdog notifier");
+
+  // Join the thread to ensure the handler has exited.
+  if (m_thread.joinable()) {
+    m_thread.join();
+  }
+
+  HAL_CleanNotifier(handle);
+}
+
+void Watchdog::Impl::UpdateAlarm() {
+  int32_t status = 0;
+  // Return if we are being destructed, or were not created successfully
+  auto notifier = m_notifier.load();
+  if (notifier == 0) {
+    return;
+  }
+  if (m_watchdogs.empty()) {
+    HAL_CancelNotifierAlarm(notifier, &status);
+  } else {
+    HAL_UpdateNotifierAlarm(
+        notifier,
+        static_cast<uint64_t>(m_watchdogs.top()->m_expirationTime.value() *
+                              1e6),
+        &status);
+  }
+  FRC_CheckErrorStatus(status, "updating watchdog notifier alarm");
+}
+
+void Watchdog::Impl::Main() {
+  for (;;) {
+    int32_t status = 0;
+    HAL_NotifierHandle notifier = m_notifier.load();
+    if (notifier == 0) {
+      break;
+    }
+    uint64_t curTime = HAL_WaitForNotifierAlarm(notifier, &status);
+    if (curTime == 0 || status != 0) {
+      break;
+    }
+
+    std::unique_lock lock(m_mutex);
+
+    if (m_watchdogs.empty()) {
+      continue;
+    }
+
+    // If the condition variable timed out, that means a Watchdog timeout
+    // has occurred, so call its timeout function.
+    auto watchdog = m_watchdogs.pop();
+
+    units::second_t now{curTime * 1e-6};
+    if (now - watchdog->m_lastTimeoutPrintTime > kMinPrintPeriod) {
+      watchdog->m_lastTimeoutPrintTime = now;
+      if (!watchdog->m_suppressTimeoutMessage) {
+        FRC_ReportWarning("Watchdog not fed within {:.6f}s",
+                          watchdog->m_timeout.value());
+      }
+    }
+
+    // Set expiration flag before calling the callback so any manipulation
+    // of the flag in the callback (e.g., calling Disable()) isn't
+    // clobbered.
+    watchdog->m_isExpired = true;
+
+    lock.unlock();
+    watchdog->m_callback();
+    lock.lock();
+
+    UpdateAlarm();
+  }
+}
+
+Watchdog::Watchdog(units::second_t timeout, std::function<void()> callback)
+    : m_timeout(timeout), m_callback(std::move(callback)), m_impl(GetImpl()) {}
+
+Watchdog::~Watchdog() {
+  try {
+    Disable();
+  } catch (const RuntimeError& e) {
+    e.Report();
+  }
+}
+
+Watchdog::Watchdog(Watchdog&& rhs) {
+  *this = std::move(rhs);
+}
+
+Watchdog& Watchdog::operator=(Watchdog&& rhs) {
+  m_impl = rhs.m_impl;
+  std::scoped_lock lock(m_impl->m_mutex);
+  m_startTime = rhs.m_startTime;
+  m_timeout = rhs.m_timeout;
+  m_expirationTime = rhs.m_expirationTime;
+  m_callback = std::move(rhs.m_callback);
+  m_lastTimeoutPrintTime = rhs.m_lastTimeoutPrintTime;
+  m_suppressTimeoutMessage = rhs.m_suppressTimeoutMessage;
+  m_tracer = std::move(rhs.m_tracer);
+  m_isExpired = rhs.m_isExpired;
+  if (m_expirationTime != 0_s) {
+    m_impl->m_watchdogs.remove(&rhs);
+    m_impl->m_watchdogs.emplace(this);
+  }
+  return *this;
+}
+
+units::second_t Watchdog::GetTime() const {
+  return Timer::GetFPGATimestamp() - m_startTime;
+}
+
+void Watchdog::SetTimeout(units::second_t timeout) {
+  m_startTime = Timer::GetFPGATimestamp();
+  m_tracer.ClearEpochs();
+
+  std::scoped_lock lock(m_impl->m_mutex);
+  m_timeout = timeout;
+  m_isExpired = false;
+
+  m_impl->m_watchdogs.remove(this);
+  m_expirationTime = m_startTime + m_timeout;
+  m_impl->m_watchdogs.emplace(this);
+  m_impl->UpdateAlarm();
+}
+
+units::second_t Watchdog::GetTimeout() const {
+  std::scoped_lock lock(m_impl->m_mutex);
+  return m_timeout;
+}
+
+bool Watchdog::IsExpired() const {
+  std::scoped_lock lock(m_impl->m_mutex);
+  return m_isExpired;
+}
+
+void Watchdog::AddEpoch(std::string_view epochName) {
+  m_tracer.AddEpoch(epochName);
+}
+
+void Watchdog::PrintEpochs() {
+  m_tracer.PrintEpochs();
+}
+
+void Watchdog::Reset() {
+  Enable();
+}
+
+void Watchdog::Enable() {
+  m_startTime = Timer::GetFPGATimestamp();
+  m_tracer.ClearEpochs();
+
+  std::scoped_lock lock(m_impl->m_mutex);
+  m_isExpired = false;
+
+  m_impl->m_watchdogs.remove(this);
+  m_expirationTime = m_startTime + m_timeout;
+  m_impl->m_watchdogs.emplace(this);
+  m_impl->UpdateAlarm();
+}
+
+void Watchdog::Disable() {
+  std::scoped_lock lock(m_impl->m_mutex);
+
+  if (m_expirationTime != 0_s) {
+    m_impl->m_watchdogs.remove(this);
+    m_expirationTime = 0_s;
+    m_impl->UpdateAlarm();
+  }
+}
+
+void Watchdog::SuppressTimeoutMessage(bool suppress) {
+  m_suppressTimeoutMessage = suppress;
+}
+
+bool Watchdog::operator>(const Watchdog& rhs) const {
+  return m_expirationTime > rhs.m_expirationTime;
+}
+
+Watchdog::Impl* Watchdog::GetImpl() {
+  static Impl inst;
+  return &inst;
+}