[wpilib] Add timestamp getters with configurable time base (#7378)

wpilibc/src/main/native/cpp/Alert.cpp

@@ -149,7 +149,7 @@ void Alert::Set(bool active) {
   }
 
   if (active) {
-    m_activeStartTime = frc::RobotController::GetFPGATime();
+    m_activeStartTime = frc::RobotController::GetTime();
     m_activeAlerts->emplace(m_activeStartTime, m_text);
   } else {
     m_activeAlerts->erase({m_activeStartTime, m_text});

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

@@ -678,7 +678,7 @@ void DriverStation::StartDataLog(wpi::log::DataLog& log, bool logJoysticks) {
 void ReportJoystickUnpluggedErrorV(fmt::string_view format,
                                    fmt::format_args args) {
   auto& inst = GetInstance();
-  auto currentTime = Timer::GetFPGATimestamp();
+  auto currentTime = Timer::GetTimestamp();
   if (currentTime > inst.nextMessageTime) {
     ReportErrorV(err::Error, "", 0, "", format, args);
     inst.nextMessageTime = currentTime + kJoystickUnpluggedMessageInterval;
@@ -689,7 +689,7 @@ void ReportJoystickUnpluggedWarningV(fmt::string_view format,
                                      fmt::format_args args) {
   auto& inst = GetInstance();
   if (DriverStation::IsFMSAttached() || !inst.silenceJoystickWarning) {
-    auto currentTime = Timer::GetFPGATimestamp();
+    auto currentTime = Timer::GetTimestamp();
     if (currentTime > inst.nextMessageTime) {
       ReportErrorV(warn::Warning, "", 0, "", format, args);
       inst.nextMessageTime = currentTime + kJoystickUnpluggedMessageInterval;

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

@@ -4,6 +4,7 @@
 
 #include "frc/RobotController.h"
 
+#include <functional>
 #include <string>
 
 #include <hal/CAN.h>
@@ -15,6 +16,10 @@
 
 using namespace frc;
 
+std::function<uint64_t()> RobotController::m_timeSource = [] {
+  return RobotController::GetFPGATime();
+};
+
 int RobotController::GetFPGAVersion() {
   int32_t status = 0;
   int version = HAL_GetFPGAVersion(&status);
@@ -49,6 +54,14 @@ 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);

wpilibc/src/main/native/cpp/TimedRobot.cpp

@@ -46,6 +46,8 @@ void TimedRobot::StartCompetition() {
       break;
     }
 
+    m_loopStartTimeUs = RobotController::GetFPGATime();
+
     callback.func();
 
     // Increment the expiration time by the number of full periods it's behind
@@ -97,6 +99,10 @@ TimedRobot::~TimedRobot() {
   }
 }
 
+uint64_t TimedRobot::GetLoopStartTime() {
+  return m_loopStartTimeUs;
+}
+
 void TimedRobot::AddPeriodic(std::function<void()> callback,
                              units::second_t period, units::second_t offset) {
   m_callbacks.emplace(

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

@@ -36,7 +36,7 @@ Timer::Timer() {
 
 units::second_t Timer::Get() const {
   if (m_running) {
-    return (GetFPGATimestamp() - m_startTime) + m_accumulatedTime;
+    return (GetTimestamp() - m_startTime) + m_accumulatedTime;
   } else {
     return m_accumulatedTime;
   }
@@ -44,12 +44,12 @@ units::second_t Timer::Get() const {
 
 void Timer::Reset() {
   m_accumulatedTime = 0_s;
-  m_startTime = GetFPGATimestamp();
+  m_startTime = GetTimestamp();
 }
 
 void Timer::Start() {
   if (!m_running) {
-    m_startTime = GetFPGATimestamp();
+    m_startTime = GetTimestamp();
     m_running = true;
   }
 }
@@ -88,6 +88,10 @@ 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};