diff --git a/wpilibc/src/main/native/cppcs/RobotBase.cpp b/wpilibc/src/main/native/cppcs/RobotBase.cpp index 58f1ec81b5..69dc19e9b4 100644 --- a/wpilibc/src/main/native/cppcs/RobotBase.cpp +++ b/wpilibc/src/main/native/cppcs/RobotBase.cpp @@ -146,6 +146,22 @@ class WPILibMathShared : public wpi::math::MathShared { HAL_Report(HALUsageReporting::kResourceType_PathWeaverTrajectory, count); break; + case wpi::math::MathUsageId::kController_LinearQuadraticRegulator: + HAL_Report(HALUsageReporting::kResourceType_LinearQuadraticRegulator, + count); + break; + case wpi::math::MathUsageId::kEstimator_KalmanFilter: + HAL_Report(HALUsageReporting::kResourceType_KalmanFilter, count); + break; + case wpi::math::MathUsageId::kEstimator_PoseEstimator: + HAL_Report(HALUsageReporting::kResourceType_PoseEstimator, count); + break; + case wpi::math::MathUsageId::kEstimator_PoseEstimator3d: + HAL_Report(HALUsageReporting::kResourceType_PoseEstimator3d, count); + break; + case wpi::math::MathUsageId::kSystem_LinearSystemLoop: + HAL_Report(HALUsageReporting::kResourceType_LinearSystemLoop, count); + break; } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java index 0cf6403196..02b9c84137 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java @@ -126,6 +126,16 @@ public abstract class RobotBase implements AutoCloseable { HAL.report(tResourceType.kResourceType_BangBangController, count); case kTrajectory_PathWeaver -> HAL.report(tResourceType.kResourceType_PathWeaverTrajectory, count); + case kController_LinearQuadraticRegulator -> + HAL.report(tResourceType.kResourceType_LinearQuadraticRegulator, count); + case kEstimator_KalmanFilter -> + HAL.report(tResourceType.kResourceType_KalmanFilter, count); + case kEstimator_PoseEstimator -> + HAL.report(tResourceType.kResourceType_PoseEstimator, count); + case kEstimator_PoseEstimator3d -> + HAL.report(tResourceType.kResourceType_PoseEstimator3d, count); + case kSystem_LinearSystemLoop -> + HAL.report(tResourceType.kResourceType_LinearSystemLoop, count); default -> { // NOP } diff --git a/wpimath/src/main/java/edu/wpi/first/math/MathUsageId.java b/wpimath/src/main/java/edu/wpi/first/math/MathUsageId.java index 94c0056743..40c11b10b4 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/MathUsageId.java +++ b/wpimath/src/main/java/edu/wpi/first/math/MathUsageId.java @@ -41,4 +41,19 @@ public enum MathUsageId { /** PathWeaver Trajectory. */ kTrajectory_PathWeaver, + + /** Linear Quadratic Regulator. */ + kController_LinearQuadraticRegulator, + + /** Kalman Filter. */ + kEstimator_KalmanFilter, + + /** Pose Estimator. */ + kEstimator_PoseEstimator, + + /** 3D Pose Estimator. */ + kEstimator_PoseEstimator3d, + + /** Linear System Loop. */ + kSystem_LinearSystemLoop, } diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java index 7b06a50340..7d771e1b88 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java @@ -5,6 +5,8 @@ package edu.wpi.first.math.controller; import edu.wpi.first.math.DARE; +import edu.wpi.first.math.MathSharedStore; +import edu.wpi.first.math.MathUsageId; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Num; import edu.wpi.first.math.StateSpaceUtil; @@ -124,6 +126,8 @@ public class LinearQuadraticRegulator(new SimpleMatrix(B.getNumCols(), 1)); reset(); + MathSharedStore.getMathShared() + .reportUsage(MathUsageId.kController_LinearQuadraticRegulator, 1); } /** @@ -163,6 +167,8 @@ public class LinearQuadraticRegulator(new SimpleMatrix(B.getNumCols(), 1)); reset(); + MathSharedStore.getMathShared() + .reportUsage(MathUsageId.kController_LinearQuadraticRegulator, 1); } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java index fa8f9245e9..0182aea58c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java @@ -5,6 +5,8 @@ package edu.wpi.first.math.estimator; import edu.wpi.first.math.DARE; +import edu.wpi.first.math.MathSharedStore; +import edu.wpi.first.math.MathUsageId; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.Num; @@ -163,6 +165,8 @@ public class ExtendedKalmanFilter(DARE.dare(discA.transpose(), C.transpose(), discQ, discR)); reset(); + + MathSharedStore.getMathShared().reportUsage(MathUsageId.kEstimator_KalmanFilter, 1); } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java index 4e9bf4b3e8..f257eff66c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java @@ -5,6 +5,7 @@ package edu.wpi.first.math.estimator; import edu.wpi.first.math.MathSharedStore; +import edu.wpi.first.math.MathUsageId; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; @@ -79,6 +80,7 @@ public class PoseEstimator { m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0)); } setVisionMeasurementStdDevs(visionMeasurementStdDevs); + MathSharedStore.getMathShared().reportUsage(MathUsageId.kEstimator_PoseEstimator, 1); } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java index 463cb9481c..a48dd5030d 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java @@ -5,6 +5,7 @@ package edu.wpi.first.math.estimator; import edu.wpi.first.math.MathSharedStore; +import edu.wpi.first.math.MathUsageId; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; @@ -87,6 +88,7 @@ public class PoseEstimator3d { m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0)); } setVisionMeasurementStdDevs(visionMeasurementStdDevs); + MathSharedStore.getMathShared().reportUsage(MathUsageId.kEstimator_PoseEstimator3d, 1); } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/SteadyStateKalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/SteadyStateKalmanFilter.java index 4b5ca7bc65..454582f526 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/SteadyStateKalmanFilter.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/SteadyStateKalmanFilter.java @@ -5,6 +5,8 @@ package edu.wpi.first.math.estimator; import edu.wpi.first.math.DARE; +import edu.wpi.first.math.MathSharedStore; +import edu.wpi.first.math.MathUsageId; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.Num; @@ -107,6 +109,7 @@ public class SteadyStateKalmanFilter(S.getStorage().solve(C.times(P).getStorage()).transpose()); reset(); + MathSharedStore.getMathShared().reportUsage(MathUsageId.kEstimator_KalmanFilter, 4); } /** Resets the observer. */ diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java index 1bc474b4c1..12671f9e74 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java @@ -4,6 +4,8 @@ package edu.wpi.first.math.estimator; +import edu.wpi.first.math.MathSharedStore; +import edu.wpi.first.math.MathUsageId; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.Num; @@ -163,6 +165,7 @@ public class UnscentedKalmanFilter(states); reset(); + MathSharedStore.getMathShared().reportUsage(MathUsageId.kEstimator_KalmanFilter, 3); } static diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystemLoop.java b/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystemLoop.java index 78cace9a21..126ae38475 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystemLoop.java +++ b/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystemLoop.java @@ -4,6 +4,8 @@ package edu.wpi.first.math.system; +import edu.wpi.first.math.MathSharedStore; +import edu.wpi.first.math.MathUsageId; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Num; import edu.wpi.first.math.StateSpaceUtil; @@ -132,6 +134,7 @@ public class LinearSystemLoop(new SimpleMatrix(controller.getK().getNumCols(), 1)); reset(m_nextR); + MathSharedStore.getMathShared().reportUsage(MathUsageId.kSystem_LinearSystemLoop, 1); } /** diff --git a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h index 10a2862683..c1aa71caa4 100644 --- a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h +++ b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h @@ -135,6 +135,8 @@ class LinearQuadraticRegulator { } Reset(); + wpi::math::MathSharedStore::ReportUsage( + wpi::math::MathUsageId::kController_LinearQuadraticRegulator, 1); } /** @@ -194,6 +196,8 @@ class LinearQuadraticRegulator { } Reset(); + wpi::math::MathSharedStore::ReportUsage( + wpi::math::MathUsageId::kController_LinearQuadraticRegulator, 1); } LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default; diff --git a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h index 22eddbc281..e9b44dceca 100644 --- a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h +++ b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h @@ -137,6 +137,8 @@ class ExtendedKalmanFilter { m_initP = StateMatrix::Zero(); } m_P = m_initP; + wpi::math::MathSharedStore::ReportUsage( + wpi::math::MathUsageId::kEstimator_KalmanFilter, 2); } /** @@ -221,6 +223,8 @@ class ExtendedKalmanFilter { m_initP = StateMatrix::Zero(); } m_P = m_initP; + wpi::math::MathSharedStore::ReportUsage( + wpi::math::MathUsageId::kEstimator_KalmanFilter, 2); } /** diff --git a/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h index aa169368bb..71684b52c7 100644 --- a/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h +++ b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h @@ -118,6 +118,8 @@ class KalmanFilter { } Reset(); + wpi::math::MathSharedStore::ReportUsage( + wpi::math::MathUsageId::kEstimator_KalmanFilter, 1); } /** diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h index 1aaf4a39d8..de091d4acb 100644 --- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h @@ -70,6 +70,8 @@ class WPILIB_DLLEXPORT PoseEstimator { } SetVisionMeasurementStdDevs(visionMeasurementStdDevs); + wpi::math::MathSharedStore::ReportUsage( + wpi::math::MathUsageId::kEstimator_PoseEstimator, 1); } /** diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h index ecf9ccbf88..e33e71b356 100644 --- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h +++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h @@ -76,6 +76,8 @@ class WPILIB_DLLEXPORT PoseEstimator3d { } SetVisionMeasurementStdDevs(visionMeasurementStdDevs); + wpi::math::MathSharedStore::ReportUsage( + wpi::math::MathUsageId::kEstimator_PoseEstimator3d, 1); } /** diff --git a/wpimath/src/main/native/include/frc/estimator/SteadyStateKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/SteadyStateKalmanFilter.h index 734c70c7e0..fd36f9fd75 100644 --- a/wpimath/src/main/native/include/frc/estimator/SteadyStateKalmanFilter.h +++ b/wpimath/src/main/native/include/frc/estimator/SteadyStateKalmanFilter.h @@ -149,6 +149,8 @@ class SteadyStateKalmanFilter { } Reset(); + wpi::math::MathSharedStore::ReportUsage( + wpi::math::MathUsageId::kEstimator_KalmanFilter, 4); } SteadyStateKalmanFilter(SteadyStateKalmanFilter&&) = default; diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h index 21838870a8..b4e37d7859 100644 --- a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h +++ b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h @@ -107,6 +107,8 @@ class UnscentedKalmanFilter { m_dt = dt; Reset(); + wpi::math::MathSharedStore::ReportUsage( + wpi::math::MathUsageId::kEstimator_KalmanFilter, 3); } /** @@ -166,6 +168,8 @@ class UnscentedKalmanFilter { m_dt = dt; Reset(); + wpi::math::MathSharedStore::ReportUsage( + wpi::math::MathUsageId::kEstimator_KalmanFilter, 3); } /** diff --git a/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h b/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h index c61531ae08..f4b6e39326 100644 --- a/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h +++ b/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h @@ -129,6 +129,8 @@ class LinearSystemLoop { m_clampFunc(clampFunction) { m_nextR.setZero(); Reset(m_nextR); + wpi::math::MathSharedStore::ReportUsage( + wpi::math::MathUsageId::kSystem_LinearSystemLoop, 1); } LinearSystemLoop(LinearSystemLoop&&) = default; diff --git a/wpimath/src/main/native/include/wpimath/MathShared.h b/wpimath/src/main/native/include/wpimath/MathShared.h index 4394b707a7..792b0d20b6 100644 --- a/wpimath/src/main/native/include/wpimath/MathShared.h +++ b/wpimath/src/main/native/include/wpimath/MathShared.h @@ -26,6 +26,11 @@ enum class MathUsageId { kController_ProfiledPIDController, kController_BangBangController, kTrajectory_PathWeaver, + kController_LinearQuadraticRegulator, + kEstimator_KalmanFilter, + kEstimator_PoseEstimator, + kEstimator_PoseEstimator3d, + kSystem_LinearSystemLoop, }; class WPILIB_DLLEXPORT MathShared {