[wpimath] SwerveDrivePoseEstimator: Fix stationary module emitting error when calculating angle in ToSwerveModuleStates (#7175)

Author: bryceroethel

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;

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;