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] Reset prev_time on pose estimator reset (#4283)

Browse Source
This commit is contained in:
Prateek Machiraju
2022-06-02 21:21:42 -07:00
committed by GitHub
parent 8b6df88783
commit d9f9cd1140
6 changed files with 12 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -226,6 +226,8 @@ public class DifferentialDrivePoseEstimator {
m_observer.setXhat(fillStateVector(poseMeters, 0.0, 0.0));
m_prevTimeSeconds = -1;
m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle);
m_previousAngle = poseMeters.getRotation();
}

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

@@ -190,6 +190,8 @@ public class MecanumDrivePoseEstimator {
m_observer.setXhat(StateSpaceUtil.poseTo3dVector(poseMeters));
m_prevTimeSeconds = -1;
m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle);
m_previousAngle = poseMeters.getRotation();
}

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

@@ -188,6 +188,8 @@ public class SwerveDrivePoseEstimator {
m_observer.setXhat(StateSpaceUtil.poseTo3dVector(poseMeters));
m_prevTimeSeconds = -1;
m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle);
m_previousAngle = poseMeters.getRotation();
}

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

@@ -58,6 +58,8 @@ void DifferentialDrivePoseEstimator::ResetPosition(
m_observer.SetXhat(FillStateVector(pose, 0_m, 0_m));
m_prevTime = -1_s;
m_gyroOffset = GetEstimatedPosition().Rotation() - gyroAngle;
m_previousAngle = pose.Rotation();
}

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

@@ -59,6 +59,8 @@ void frc::MecanumDrivePoseEstimator::ResetPosition(
m_observer.SetXhat(PoseTo3dVector(pose));
m_prevTime = -1_s;
m_gyroOffset = pose.Rotation() - gyroAngle;
m_previousAngle = pose.Rotation();
}

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

@@ -127,6 +127,8 @@ class SwerveDrivePoseEstimator {
m_observer.SetXhat(PoseTo3dVector(pose));
m_prevTime = -1_s;
m_gyroOffset = pose.Rotation() - gyroAngle;
m_previousAngle = pose.Rotation();
}