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

[wpimath] Deduplicate angle modulus functions (#2998)

Browse Source 
frc::NormalizeAngle(), units::math::NormalizeAngle(), and
frc::GetModulusError() were replaced with frc::InputModulus() and
frc::AngleModulus().

They were placed in wpimath/src/main/native/include/frc/MathUtil.h for
C++ and wpimath/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java
for Java.
This commit is contained in:
Tyler Veness
2021-01-01 16:22:00 -08:00
committed by GitHub
parent bf8c0da4be
commit 62f0f8190d
23 changed files with 210 additions and 219 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
wpimath/src/main/native/include/frc/estimator/AngleStatistics.h
View File

@@ -7,17 +7,9 @@
#include <wpi/math>
#include "Eigen/Core"
#include "frc/MathUtil.h"
namespace frc {
inline double NormalizeAngle(double angle) {
static constexpr double tau = 2 * wpi::math::pi;
angle -= std::floor(angle / tau) * tau;
if (angle > wpi::math::pi) {
angle -= tau;
}
return angle;
}
/**
* Subtracts a and b while normalizing the resulting value in the selected row
@@ -32,7 +24,8 @@ Eigen::Matrix<double, States, 1> AngleResidual(
const Eigen::Matrix<double, States, 1>& a,
const Eigen::Matrix<double, States, 1>& b, int angleStateIdx) {
Eigen::Matrix<double, States, 1> ret = a - b;
ret[angleStateIdx] = NormalizeAngle(ret[angleStateIdx]);
ret[angleStateIdx] =
AngleModulus(units::radian_t{ret[angleStateIdx]}).to<double>();
return ret;
}
@@ -65,7 +58,8 @@ Eigen::Matrix<double, States, 1> AngleAdd(
const Eigen::Matrix<double, States, 1>& a,
const Eigen::Matrix<double, States, 1>& b, int angleStateIdx) {
Eigen::Matrix<double, States, 1> ret = a + b;
ret[angleStateIdx] = NormalizeAngle(ret[angleStateIdx]);
ret[angleStateIdx] =
InputModulus(ret[angleStateIdx], -wpi::math::pi, wpi::math::pi);
return ret;
}
@@ -122,4 +116,5 @@ AngleMean(int angleStateIdx) {
return AngleMean<CovDim, States>(sigmas, Wm, angleStateIdx);
};
}
} // namespace frc