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 order of setting gyro offset in pose estimators (#3176)

Browse Source 
The gyro offset should be determined from the desired initial pose, not the current pose. This fix reflects the behavior of the odometry classes and the C++ holonomic pose estimators.
This commit is contained in:
Claudius Tewari
2021-02-16 18:04:38 -08:00
committed by GitHub
parent d241bc81ae
commit d1c7032dec
4 changed files with 4 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimator.java
View File

@@ -217,8 +217,8 @@ public class DifferentialDrivePoseEstimator {
*/
public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
m_previousAngle = poseMeters.getRotation();
m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle);
m_observer.setXhat(fillStateVector(poseMeters, 0.0, 0.0));
m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle);
}
/**

2
wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/MecanumDrivePoseEstimator.java
View File

@@ -182,8 +182,8 @@ public class MecanumDrivePoseEstimator {
*/
public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
m_previousAngle = poseMeters.getRotation();
m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle);
m_observer.setXhat(StateSpaceUtil.poseTo3dVector(poseMeters));
m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle);
}
/**

2
wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimator.java
View File

@@ -182,8 +182,8 @@ public class SwerveDrivePoseEstimator {
*/
public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
m_previousAngle = poseMeters.getRotation();
m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle);
m_observer.setXhat(StateSpaceUtil.poseTo3dVector(poseMeters));
m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle);
}
/**