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] SwerveDrivePoseEstimator: Fix stationary module emitting error when calculating angle in ToSwerveModuleStates (#7175)

Browse Source
This commit is contained in:
Bryce Roethel
2024-10-11 01:05:49 -04:00
committed by GitHub
parent 2085ab3d47
commit d97a749d84
2 changed files with 3 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
View File

@@ -66,7 +66,8 @@ SwerveDriveKinematics<NumModules>::ToSwerveModuleStates(
units::meters_per_second_t y{moduleStateMatrix(i * 2 + 1, 0)};
auto speed = units::math::hypot(x, y);
Rotation2d rotation{x.value(), y.value()};
auto rotation = speed > 1e-6_mps ? Rotation2d{x.value(), y.value()}
: m_moduleHeadings[i];
moduleStates[i] = {speed, rotation};
m_moduleHeadings[i] = rotation;