Unify WPI_Now and HAL_GetFPGATime. (#743)

Depends on wpilibsuite/wpiutil#56.

hal/src/main/native/athena/HAL.cpp

@@ -20,6 +20,7 @@
 #include <FRC_NetworkCommunication/LoadOut.h>
 #include <llvm/raw_ostream.h>
 #include <support/mutex.h>
+#include <support/timestamp.h>
 
 #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;
 }

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();
   }

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

@@ -19,19 +19,15 @@ static std::atomic<bool> programStarted{false};
 static std::atomic<uint64_t> 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