Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-28 02:11:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
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>
This commit is contained in:
Jonah
2023-02-17 17:53:17 -05:00
committed by GitHub
parent 9cc14bbb43
commit e9a7bed988
16 changed files with 76 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
View File

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

8
wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h
View File

@@ -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:

7
wpimath/src/main/native/include/wpimath/MathShared.h
View File

@@ -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