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

[wpimath, wpiutil] Add wpi::array for compile time size checking (#3087)

Browse Source 
The wpimath APIs use std::array, which doesn't do size checking. Passing
an array with the wrong size can result in uninitialized elements
instead of a compilation error.

This is a breaking change but is worthwhile to avoid hard-to-debug errors.
This commit is contained in:
Tyler Veness
2021-01-16 20:26:17 -08:00
committed by GitHub
parent d6ed20c1e4
commit f393989a5b
25 changed files with 227 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
View File

@@ -13,9 +13,9 @@ using namespace frc;
DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
const Rotation2d& gyroAngle, const Pose2d& initialPose,
const std::array<double, 5>& stateStdDevs,
const std::array<double, 3>& localMeasurementStdDevs,
const std::array<double, 3>& visionMeasurmentStdDevs,
const wpi::array<double, 5>& stateStdDevs,
const wpi::array<double, 3>& localMeasurementStdDevs,
const wpi::array<double, 3>& visionMeasurmentStdDevs,
units::second_t nominalDt)
: m_observer(
&DifferentialDrivePoseEstimator::F,
@@ -46,7 +46,7 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
}
void DifferentialDrivePoseEstimator::SetVisionMeasurementStdDevs(
const std::array<double, 3>& visionMeasurmentStdDevs) {
const wpi::array<double, 3>& visionMeasurmentStdDevs) {
// Create R (covariances) for vision measurements.
Eigen::Matrix<double, 3, 3> visionContR =
frc::MakeCovMatrix(visionMeasurmentStdDevs);
@@ -126,9 +126,9 @@ Eigen::Matrix<double, 5, 1> DifferentialDrivePoseEstimator::F(
}
template <int Dim>
std::array<double, Dim> DifferentialDrivePoseEstimator::StdDevMatrixToArray(
wpi::array<double, Dim> DifferentialDrivePoseEstimator::StdDevMatrixToArray(
const Eigen::Matrix<double, Dim, 1>& stdDevs) {
std::array<double, Dim> array;
wpi::array<double, Dim> array;
for (size_t i = 0; i < Dim; ++i) {
array[i] = stdDevs(i);
}

8
wpimath/src/main/native/cpp/estimator/KalmanFilter.cpp
View File

@@ -7,14 +7,14 @@
namespace frc {
KalmanFilter<1, 1, 1>::KalmanFilter(
LinearSystem<1, 1, 1>& plant, const std::array<double, 1>& stateStdDevs,
const std::array<double, 1>& measurementStdDevs, units::second_t dt)
LinearSystem<1, 1, 1>& plant, const wpi::array<double, 1>& stateStdDevs,
const wpi::array<double, 1>& measurementStdDevs, units::second_t dt)
: detail::KalmanFilterImpl<1, 1, 1>{plant, stateStdDevs, measurementStdDevs,
dt} {}
KalmanFilter<2, 1, 1>::KalmanFilter(
LinearSystem<2, 1, 1>& plant, const std::array<double, 2>& stateStdDevs,
const std::array<double, 1>& measurementStdDevs, units::second_t dt)
LinearSystem<2, 1, 1>& plant, const wpi::array<double, 2>& stateStdDevs,
const wpi::array<double, 1>& measurementStdDevs, units::second_t dt)
: detail::KalmanFilterImpl<2, 1, 1>{plant, stateStdDevs, measurementStdDevs,
dt} {}

8
wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
View File

@@ -14,9 +14,9 @@ using namespace frc;
frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
const Rotation2d& gyroAngle, const Pose2d& initialPose,
MecanumDriveKinematics kinematics,
const std::array<double, 3>& stateStdDevs,
const std::array<double, 1>& localMeasurementStdDevs,
const std::array<double, 3>& visionMeasurementStdDevs,
const wpi::array<double, 3>& stateStdDevs,
const wpi::array<double, 1>& localMeasurementStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs,
units::second_t nominalDt)
: m_observer([](const Eigen::Matrix<double, 3, 1>& x,
const Eigen::Matrix<double, 3, 1>& u) { return u; },
@@ -51,7 +51,7 @@ frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
}
void frc::MecanumDrivePoseEstimator::SetVisionMeasurementStdDevs(
const std::array<double, 3>& visionMeasurmentStdDevs) {
const wpi::array<double, 3>& visionMeasurmentStdDevs) {
// Create R (covariances) for vision measurements.
Eigen::Matrix<double, 3, 3> visionContR =
frc::MakeCovMatrix(visionMeasurmentStdDevs);