Updated to include gyro and accelerometer, and Rotation3d offset.

swervelib/imu/PigeonSwerve.java

@@ -1,7 +1,11 @@
 package swervelib.imu;
 
 import com.ctre.phoenix.sensors.WPI_PigeonIMU;
+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.smartdashboard.SmartDashboard;
+import java.util.Optional;
 
 /**
  * SwerveIMU interface for the Pigeon.
@@ -65,6 +69,33 @@ public class PigeonSwerve extends SwerveIMU
     imu.getYawPitchRoll(yprArray);
   }
 
+  /**
+   * Fetch the {@link Rotation3d} from the IMU. Robot relative.
+   *
+   * @return {@link Rotation3d} from the IMU.
+   */
+  @Override
+  public Rotation3d getRotation3d()
+  {
+    double[] wxyz = new double[4];
+    imu.get6dQuaternion(wxyz);
+    return new Rotation3d(new Quaternion(wxyz[0], wxyz[1], wxyz[2], wxyz[3]));
+  }
+
+  /**
+   * 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()
+  {
+    short[] initial = new short[3];
+    imu.getBiasedAccelerometer(initial);
+    return Optional.of(new Translation3d(initial[0], initial[1], initial[2]).times(9.81 / 16384.0));
+  }
+
   /**
    * Get the instantiated IMU object.
    *