Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +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

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

@@ -94,6 +94,13 @@ Pose2d DifferentialDrivePoseEstimator::GetEstimatedPosition() const {
void DifferentialDrivePoseEstimator::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);

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

@@ -105,6 +105,13 @@ Pose2d frc::MecanumDrivePoseEstimator::GetEstimatedPosition() const {
void frc::MecanumDrivePoseEstimator::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);