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] Fix C++ pose estimator poseEstimate initialization (#7249)

Browse Source
This commit is contained in:
Joseph Eng
2024-10-21 22:29:04 -07:00
committed by GitHub
parent 40af8db28a
commit 6745fc7c2f
7 changed files with 42 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -24,4 +24,6 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
: PoseEstimator<DifferentialDriveWheelSpeeds,
DifferentialDriveWheelPositions>(
kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
m_odometryImpl{gyroAngle, leftDistance, rightDistance, initialPose} {}
m_odometryImpl{gyroAngle, leftDistance, rightDistance, initialPose} {
ResetPose(m_odometryImpl.GetPose());
}

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

@@ -26,4 +26,6 @@ frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
const wpi::array<double, 3>& visionMeasurementStdDevs)
: PoseEstimator<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions>(
kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
m_odometryImpl(kinematics, gyroAngle, wheelPositions, initialPose) {}
m_odometryImpl(kinematics, gyroAngle, wheelPositions, initialPose) {
ResetPose(m_odometryImpl.GetPose());
}