[wpimath] SwerveDrivePoseEstimator: Fix stationary module emitting er…

…ror when calculating angle in ToSwerveModuleStates (wpilibsuite#7175)
spacey-sooty · Oct 26, 2024 · 64cda4d · 64cda4d
1 parent 9636815
commit 64cda4d
Show file tree

Hide file tree

Showing 2 changed files with 3 additions and 2 deletions.
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java
@@ -167,7 +167,7 @@ public SwerveModuleState[] toSwerveModuleStates(
       double y = moduleStatesMatrix.get(i * 2 + 1, 0);
 
       double speed = Math.hypot(x, y);
-      Rotation2d angle = new Rotation2d(x, y);
+      Rotation2d angle = speed > 1e-6 ? new Rotation2d(x, y) : m_moduleHeadings[i];
 
       moduleStates[i] = new SwerveModuleState(speed, angle);
       m_moduleHeadings[i] = angle;

diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
@@ -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;