diff --git a/hal/src/main/native/athena/HAL.cpp b/hal/src/main/native/athena/HAL.cpp index 4ec4136b1d..b8a9630215 100644 --- a/hal/src/main/native/athena/HAL.cpp +++ b/hal/src/main/native/athena/HAL.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include "HAL/ChipObject.h" #include "HAL/DriverStation.h" @@ -357,6 +358,20 @@ HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode) { HAL_InitializeDriverStation(); + // Set WPI_Now to use FPGA timestamp + wpi::SetNowImpl([]() -> uint64_t { + int32_t status = 0; + uint64_t rv = HAL_GetFPGATime(&status); + if (status != 0) { + llvm::errs() + << "Call to HAL_GetFPGATime failed." + << "Initialization might have failed. Time will not be correct\n"; + llvm::errs().flush(); + return 0u; + } + return rv; + }); + initialized = true; return true; } diff --git a/hal/src/main/native/shared/cpp/fpga_clock.cpp b/hal/src/main/native/shared/cpp/fpga_clock.cpp index fd201e1be7..917877a32c 100644 --- a/hal/src/main/native/shared/cpp/fpga_clock.cpp +++ b/hal/src/main/native/shared/cpp/fpga_clock.cpp @@ -22,7 +22,7 @@ fpga_clock::time_point fpga_clock::now() noexcept { if (status != 0) { llvm::errs() << "Call to HAL_GetFPGATime failed." - << "Initialization might have failed. Time will not be correct"; + << "Initialization might have failed. Time will not be correct\n"; llvm::errs().flush(); return epoch(); } diff --git a/hal/src/main/native/sim/MockHooks.cpp b/hal/src/main/native/sim/MockHooks.cpp index 15f1fc0aa6..5044942ae5 100644 --- a/hal/src/main/native/sim/MockHooks.cpp +++ b/hal/src/main/native/sim/MockHooks.cpp @@ -19,19 +19,15 @@ static std::atomic programStarted{false}; static std::atomic programStartTime{0}; namespace hal { -void RestartTiming() { programStartTime = wpi::Now() / 10; } +void RestartTiming() { programStartTime = wpi::Now(); } int64_t GetFPGATime() { - auto now = wpi::Now() / 10; + auto now = wpi::Now(); auto currentTime = now - programStartTime; return currentTime; } -double GetFPGATimestamp() { - auto now = wpi::Now() / 10; - auto currentTime = now - programStartTime; - return currentTime * 1.0e-6; -} +double GetFPGATimestamp() { return GetFPGATime() * 1.0e-6; } void SetProgramStarted() { programStarted = true; } } // namespace hal