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

[wpimath] Discard stale pose estimates (#5045)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Jordan McMichael
2023-02-04 01:04:30 -05:00
committed by GitHub
parent fe5d226a19
commit 4079eabe9b
14 changed files with 283 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
View File

@@ -240,14 +240,16 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
double i) const;
};
static constexpr units::second_t kBufferDuration = 1.5_s;
DifferentialDriveKinematics& m_kinematics;
DifferentialDriveOdometry m_odometry;
wpi::array<double, 3> m_q{wpi::empty_array};
Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer{
1.5_s, [this](const InterpolationRecord& start,
const InterpolationRecord& end, double t) {
kBufferDuration, [this](const InterpolationRecord& start,
const InterpolationRecord& end, double t) {
return start.Interpolate(this->m_kinematics, end, t);
}};
};

6
wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
View File

@@ -232,14 +232,16 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
double i) const;
};
static constexpr units::second_t kBufferDuration = 1.5_s;
MecanumDriveKinematics& m_kinematics;
MecanumDriveOdometry m_odometry;
wpi::array<double, 3> m_q{wpi::empty_array};
Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer{
1.5_s, [this](const InterpolationRecord& start,
const InterpolationRecord& end, double t) {
kBufferDuration, [this](const InterpolationRecord& start,
const InterpolationRecord& end, double t) {
return start.Interpolate(this->m_kinematics, end, t);
}};
};

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

@@ -170,6 +170,13 @@ class SwerveDrivePoseEstimator {
*/
void AddVisionMeasurement(const Pose2d& visionRobotPose,
units::second_t timestamp) {
// Step 0: If this measurement is old enough to be outside the pose buffer's
// timespan, skip.
if (m_poseBuffer.GetInternalBuffer().front().first - kBufferDuration >
timestamp) {
return;
}
// Step 1: Get the estimated pose from when the vision measurement was made.
auto sample = m_poseBuffer.Sample(timestamp);
@@ -374,14 +381,16 @@ class SwerveDrivePoseEstimator {
}
};
static constexpr units::second_t kBufferDuration = 1.5_s;
SwerveDriveKinematics<NumModules>& m_kinematics;
SwerveDriveOdometry<NumModules> m_odometry;
wpi::array<double, 3> m_q{wpi::empty_array};
Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer{
1.5_s, [this](const InterpolationRecord& start,
const InterpolationRecord& end, double t) {
kBufferDuration, [this](const InterpolationRecord& start,
const InterpolationRecord& end, double t) {
return start.Interpolate(this->m_kinematics, end, t);
}};
};