[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) {

wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h

@@ -18,6 +18,7 @@
 #include "frc/kinematics/SwerveDriveKinematics.h"
 #include "frc/kinematics/SwerveDriveOdometry.h"
 #include "units/time.h"
+#include "wpimath/MathShared.h"
 
 namespace frc {
 
@@ -272,7 +273,7 @@ class SwerveDrivePoseEstimator {
   Pose2d Update(
       const Rotation2d& gyroAngle,
       const wpi::array<SwerveModulePosition, NumModules>& modulePositions) {
-    return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
+    return UpdateWithTime(wpi::math::MathSharedStore::GetTimestamp(), gyroAngle,
                           modulePositions);
   }
 

wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h

@@ -10,6 +10,7 @@
 #include <wpi/timestamp.h>
 
 #include "units/time.h"
+#include "wpimath/MathShared.h"
 
 namespace frc {
 /**
@@ -45,7 +46,8 @@ class SlewRateLimiter {
       : m_positiveRateLimit{positiveRateLimit},
         m_negativeRateLimit{negativeRateLimit},
         m_prevVal{initialValue},
-        m_prevTime{units::microsecond_t(wpi::Now())} {}
+        m_prevTime{
+            units::microsecond_t(wpi::math::MathSharedStore::GetTimestamp())} {}
 
   /**
    * Creates a new SlewRateLimiter with the given positive rate limit and
@@ -77,7 +79,7 @@ class SlewRateLimiter {
    * rate.
    */
   Unit_t Calculate(Unit_t input) {
-    units::second_t currentTime = units::microsecond_t(wpi::Now());
+    units::second_t currentTime = wpi::math::MathSharedStore::GetTimestamp();
     units::second_t elapsedTime = currentTime - m_prevTime;
     m_prevVal +=
         std::clamp(input - m_prevVal, m_negativeRateLimit * elapsedTime,
@@ -94,7 +96,7 @@ class SlewRateLimiter {
    */
   void Reset(Unit_t value) {
     m_prevVal = value;
-    m_prevTime = units::microsecond_t(wpi::Now());
+    m_prevTime = wpi::math::MathSharedStore::GetTimestamp();
   }
 
  private:

wpimath/src/main/native/include/wpimath/MathShared.h

@@ -9,6 +9,8 @@
 #include <fmt/format.h>
 #include <wpi/SymbolExports.h>
 
+#include "units/time.h"
+
 namespace wpi::math {
 
 enum class MathUsageId {
@@ -31,6 +33,7 @@ class WPILIB_DLLEXPORT MathShared {
   virtual void ReportWarningV(fmt::string_view format,
                               fmt::format_args args) = 0;
   virtual void ReportUsage(MathUsageId id, int count) = 0;
+  virtual units::second_t GetTimestamp() = 0;
 
   template <typename S, typename... Args>
   inline void ReportError(const S& format, Args&&... args) {
@@ -70,6 +73,10 @@ class WPILIB_DLLEXPORT MathSharedStore {
   static void ReportUsage(MathUsageId id, int count) {
     GetMathShared().ReportUsage(id, count);
   }
+
+  static units::second_t GetTimestamp() {
+    return GetMathShared().GetTimestamp();
+  }
 };
 
 }  // namespace wpi::math