[wpimath] Add timestamp getter to MathShared (#5091)

This makes it possible to mock the timestamp for wpimath without affecting the rest of the library.

Co-authored-by: Peter Johnson <johnson.peter@gmail.com>

wpimath/src/main/native/cpp/MathShared.cpp

@@ -5,6 +5,9 @@
 #include "wpimath/MathShared.h"
 
 #include <wpi/mutex.h>
+#include <wpi/timestamp.h>
+
+#include "units/time.h"
 
 using namespace wpi::math;
 
@@ -15,6 +18,9 @@ class DefaultMathShared : public MathShared {
   void ReportWarningV(fmt::string_view format, fmt::format_args args) override {
   }
   void ReportUsage(MathUsageId id, int count) override {}
+  units::second_t GetTimestamp() override {
+    return units::second_t{wpi::Now() * 1.0e-6};
+  }
 };
 }  // namespace
 

wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp

@@ -8,6 +8,7 @@
 
 #include "frc/StateSpaceUtil.h"
 #include "frc/estimator/AngleStatistics.h"
+#include "wpimath/MathShared.h"
 
 using namespace frc;
 
@@ -153,7 +154,7 @@ void DifferentialDrivePoseEstimator::AddVisionMeasurement(
 Pose2d DifferentialDrivePoseEstimator::Update(const Rotation2d& gyroAngle,
                                               units::meter_t leftDistance,
                                               units::meter_t rightDistance) {
-  return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
+  return UpdateWithTime(wpi::math::MathSharedStore::GetTimestamp(), gyroAngle,
                         leftDistance, rightDistance);
 }
 

wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp

@@ -8,6 +8,7 @@
 
 #include "frc/StateSpaceUtil.h"
 #include "frc/estimator/AngleStatistics.h"
+#include "wpimath/MathShared.h"
 
 using namespace frc;
 
@@ -163,7 +164,7 @@ void frc::MecanumDrivePoseEstimator::AddVisionMeasurement(
 Pose2d frc::MecanumDrivePoseEstimator::Update(
     const Rotation2d& gyroAngle,
     const MecanumDriveWheelPositions& wheelPositions) {
-  return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
+  return UpdateWithTime(wpi::math::MathSharedStore::GetTimestamp(), gyroAngle,
                         wheelPositions);
 }
 

wpimath/src/main/native/cpp/filter/Debouncer.cpp

@@ -4,6 +4,8 @@
 
 #include "frc/filter/Debouncer.h"
 
+#include "wpimath/MathShared.h"
+
 using namespace frc;
 
 Debouncer::Debouncer(units::second_t debounceTime, DebounceType type)
@@ -21,11 +23,12 @@ Debouncer::Debouncer(units::second_t debounceTime, DebounceType type)
 }
 
 void Debouncer::ResetTimer() {
-  m_prevTime = units::microsecond_t(wpi::Now());
+  m_prevTime = wpi::math::MathSharedStore::GetTimestamp();
 }
 
 bool Debouncer::HasElapsed() const {
-  return units::microsecond_t(wpi::Now()) - m_prevTime >= m_debounceTime;
+  return wpi::math::MathSharedStore::GetTimestamp() - m_prevTime >=
+         m_debounceTime;
 }
 
 bool Debouncer::Calculate(bool input) {