Updated to include gyro and accelerometer, and Rotation3d offset.

swervelib/imu/NavXSwerve.java

@@ -1,9 +1,13 @@
 package swervelib.imu;
 
 import com.kauailabs.navx.frc.AHRS;
+import edu.wpi.first.math.geometry.Quaternion;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.SerialPort;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import java.util.Optional;
 
 /**
  * Communicates with the NavX as the IMU.
@@ -84,6 +88,37 @@ public class NavXSwerve extends SwerveIMU
     yprArray[2] = (gyro.getRoll() % 360);
   }
 
+  /**
+   * Fetch the {@link Rotation3d} from the IMU. Robot relative.
+   *
+   * @return {@link Rotation3d} from the IMU.
+   */
+  public Rotation3d getRotation3d()
+  {
+    return new Rotation3d(new Quaternion(gyro.getQuaternionW(),
+                                         gyro.getQuaternionX(),
+                                         gyro.getQuaternionY(),
+                                         gyro.getQuaternionZ()))
+        .minus(new Rotation3d(0, 0, Math.toRadians(yawOffset)));
+  }
+
+  /**
+   * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration isn't supported returns
+   * empty.
+   *
+   * @return {@link Translation3d} of the acceleration as an {@link Optional}.
+   */
+  @Override
+  public Optional<Translation3d> getAccel()
+  {
+    return Optional.of(
+        new Translation3d(
+            gyro.getWorldLinearAccelX(),
+            gyro.getWorldLinearAccelY(),
+            gyro.getWorldLinearAccelZ())
+            .times(9.81));
+  }
+
   /**
    * Get the instantiated IMU object.
    *