[hal, wpilib] Add systemcore IMU (#8016)

hal/src/main/java/edu/wpi/first/hal/IMUJNI.java

@@ -0,0 +1,65 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.hal;
+
+/**
+ * IMU Functions.
+ *
+ * @see "hal/IMU.h"
+ */
+public class IMUJNI extends JNIWrapper {
+  /**
+   * Get the acceleration along the axes of the IMU in meters per second squared.
+   *
+   * @param accel array of size >= 3 to place the acceleration data in. The data will be in the form
+   *     [x, y, z].
+   */
+  public static native void getIMUAcceleration(double[] accel);
+
+  /**
+   * Get the angular rate about the axes of the IMU in radians per second.
+   *
+   * @param rates array of size >= 3 to place the angular rate data in. The data will be in the form
+   *     [x, y, z].
+   */
+  public static native void getIMUGyroRates(double[] rates);
+
+  /**
+   * Get the angle about the axes of the IMU in radians.
+   *
+   * @param angles array of size >= 3 to place the angle data in. The data will be in the form [x,
+   *     y, z].
+   */
+  public static native void getIMUEulerAngles(double[] angles);
+
+  /**
+   * Get the orientation of the IMU as a quaternion.
+   *
+   * @param quat array of size >= 4 to place the quaternion data in. The data will be in the form
+   *     [w, x, y, z].
+   */
+  public static native void getIMUQuaternion(double[] quat);
+
+  /**
+   * Get the yaw value, in radians, of the IMU in the "flat" orientation.
+   *
+   * @return flat orientation yaw
+   */
+  public static native double getIMUYawFlat();
+
+  /**
+   * Get the yaw value, in radians, of the IMU in the "landscape" orientation.
+   *
+   * @return landscape orientation yaw
+   */
+  public static native double getIMUYawLandscape();
+
+  /**
+   * Get the yaw value, in radians, of the IMU in the "portrait" orientation.
+   *
+   * @return portrait orientation yaw
+   */
+  public static native double getIMUYawPortrait();
+}

hal/src/main/native/cpp/jni/IMUJNI.cpp

@@ -0,0 +1,154 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <jni.h>
+
+#include <string_view>
+
+#include <fmt/format.h>
+
+#include "HALUtil.h"
+#include "edu_wpi_first_hal_IMUJNI.h"
+#include "hal/IMU.h"
+
+using namespace hal;
+
+namespace {
+void assertArraySize(JNIEnv* env, jarray array, int minimumSize,
+                     std::string_view arrayName) {
+  jsize actualSize = env->GetArrayLength(array);
+  if (actualSize < minimumSize) {
+    ThrowIllegalArgumentException(
+        env, fmt::format("{} array too small: expected at least {}, got {}",
+                         arrayName, minimumSize, actualSize));
+  }
+}
+}  // namespace
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_hal_IMUJNI
+ * Method:    getIMUAcceleration
+ * Signature: ([D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_IMUJNI_getIMUAcceleration
+  (JNIEnv* env, jclass, jdoubleArray accel)
+{
+  assertArraySize(env, accel, 3, "accel");
+
+  int32_t status = 0;
+  HAL_Acceleration3d data;
+
+  HAL_GetIMUAcceleration(&data, &status);
+  CheckStatus(env, status);
+
+  double arr[]{data.x, data.y, data.z};
+  env->SetDoubleArrayRegion(accel, 0, 3, arr);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_IMUJNI
+ * Method:    getIMUGyroRates
+ * Signature: ([D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_IMUJNI_getIMUGyroRates
+  (JNIEnv* env, jclass, jdoubleArray rates)
+{
+  assertArraySize(env, rates, 3, "rates");
+
+  int32_t status = 0;
+  HAL_GyroRate3d data;
+
+  HAL_GetIMUGyroRates(&data, &status);
+  CheckStatus(env, status);
+
+  double arr[]{data.x, data.y, data.z};
+  env->SetDoubleArrayRegion(rates, 0, 3, arr);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_IMUJNI
+ * Method:    getIMUEulerAngles
+ * Signature: ([D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_IMUJNI_getIMUEulerAngles
+  (JNIEnv* env, jclass, jdoubleArray angles)
+{
+  assertArraySize(env, angles, 3, "angles");
+
+  int32_t status = 0;
+  HAL_EulerAngles3d data;
+
+  HAL_GetIMUEulerAngles(&data, &status);
+  CheckStatus(env, status);
+
+  double arr[]{data.x, data.y, data.z};
+  env->SetDoubleArrayRegion(angles, 0, 3, arr);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_IMUJNI
+ * Method:    getIMUQuaternion
+ * Signature: ([D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_IMUJNI_getIMUQuaternion
+  (JNIEnv* env, jclass, jdoubleArray quat)
+{
+  assertArraySize(env, quat, 4, "quat");
+
+  int32_t status = 0;
+  HAL_Quaternion data;
+
+  HAL_GetIMUQuaternion(&data, &status);
+  CheckStatus(env, status);
+
+  double arr[]{data.w, data.x, data.y, data.z};
+  env->SetDoubleArrayRegion(quat, 0, 4, arr);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_IMUJNI
+ * Method:    getIMUYawFlat
+ * Signature: ()D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_IMUJNI_getIMUYawFlat
+  (JNIEnv* env, jclass)
+{
+  int64_t timestamp;
+  return HAL_GetIMUYawFlat(&timestamp);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_IMUJNI
+ * Method:    getIMUYawLandscape
+ * Signature: ()D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_IMUJNI_getIMUYawLandscape
+  (JNIEnv* env, jclass)
+{
+  int64_t timestamp;
+  return HAL_GetIMUYawLandscape(&timestamp);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_IMUJNI
+ * Method:    getIMUYawPortrait
+ * Signature: ()D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_IMUJNI_getIMUYawPortrait
+  (JNIEnv* env, jclass)
+{
+  int64_t timestamp;
+  return HAL_GetIMUYawPortrait(&timestamp);
+}
+
+}  // extern "C"

hal/src/main/native/include/hal/IMU.h

@@ -0,0 +1,79 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <stdint.h>
+
+#include "hal/IMUTypes.h"
+
+/**
+ * @defgroup hal_imu IMU Functions
+ * @ingroup hal_capi
+ * @{
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Get the acceleration along the axes of the IMU in meters per second squared.
+ *
+ * @param[out] accel the acceleration data
+ * @param[out] status the error code, or 0 for success
+ */
+void HAL_GetIMUAcceleration(HAL_Acceleration3d* accel, int32_t* status);
+
+/**
+ * Get the angular rate about the axes of the IMU in radians per second.
+ *
+ * @param[out] rates the angular rate data
+ * @param[out] status the error code, or 0 for success
+ */
+void HAL_GetIMUGyroRates(HAL_GyroRate3d* rates, int32_t* status);
+
+/**
+ * Get the angle about the axes of the IMU in radians.
+ *
+ * @param[out] angles the angle data
+ * @param[out] status the error code, or 0 for success
+ */
+void HAL_GetIMUEulerAngles(HAL_EulerAngles3d* angles, int32_t* status);
+
+/**
+ * Get the orientation of the IMU as a quaternion.
+ *
+ * @param[out] quat the quaternion
+ * @param[out] status the error code, or 0 for success
+ */
+void HAL_GetIMUQuaternion(HAL_Quaternion* quat, int32_t* status);
+
+/**
+ * Get the yaw value, in radians, of the IMU in the "flat" orientation.
+ *
+ * @param[out] timestamp the timestamp of the sample
+ * @return flat orientation yaw
+ */
+double HAL_GetIMUYawFlat(int64_t* timestamp);
+
+/**
+ * Get the yaw value, in radians, of the IMU in the "landscape" orientation.
+ *
+ * @param[out] timestamp the timestamp of the sample
+ * @return landscape orientation yaw
+ */
+double HAL_GetIMUYawLandscape(int64_t* timestamp);
+
+/**
+ * Get the yaw value, in radians, of the IMU in the "portrait" orientation.
+ *
+ * @param[out] timestamp the timestamp of the sample
+ * @return portrait orientation yaw
+ */
+double HAL_GetIMUYawPortrait(int64_t* timestamp);
+#ifdef __cplusplus
+}  // extern "C"
+#endif
+/** @} */

hal/src/main/native/include/hal/IMUTypes.h

@@ -0,0 +1,48 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct HAL_Quaternion {
+  int64_t timestamp;
+  double w;
+  double x;
+  double y;
+  double z;
+};
+typedef struct HAL_Quaternion HAL_Quaternion;
+
+struct HAL_Acceleration3d {
+  int64_t timestamp;
+  double x;
+  double y;
+  double z;
+};
+typedef struct HAL_Acceleration3d HAL_Acceleration3d;
+
+struct HAL_GyroRate3d {
+  int64_t timestamp;
+  double x;
+  double y;
+  double z;
+};
+typedef struct HAL_GyroRate3d HAL_GyroRate3d;
+
+struct HAL_EulerAngles3d {
+  int64_t timestamp;
+  double x;
+  double y;
+  double z;
+};
+typedef struct HAL_EulerAngles3d HAL_EulerAngles3d;
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif

hal/src/main/native/sim/IMU.cpp

@@ -0,0 +1,30 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "hal/IMU.h"
+
+extern "C" {
+// TODO(Ryan) implement sim
+void HAL_GetIMUAcceleration(HAL_Acceleration3d* accel, int32_t* status) {
+  *accel = {};
+}
+void HAL_GetIMUGyroRates(HAL_GyroRate3d* rate, int32_t* status) {
+  *rate = {};
+}
+void HAL_GetIMUEulerAngles(HAL_EulerAngles3d* angles, int32_t* status) {
+  *angles = {};
+}
+void HAL_GetIMUQuaternion(HAL_Quaternion* quat, int32_t* status) {
+  *quat = {};
+}
+double HAL_GetIMUYawFlat(int64_t* timestamp) {
+  return 0;
+}
+double HAL_GetIMUYawLandscape(int64_t* timestamp) {
+  return 0;
+}
+double HAL_GetIMUYawPortrait(int64_t* timestamp) {
+  return 0;
+}
+}  // extern "C"

hal/src/main/native/systemcore/HAL.cpp

@@ -59,6 +59,7 @@ void InitializeHAL() {
   InitializeEncoder();
   InitializeFRCDriverStation();
   InitializeI2C();
+  InitializeIMU();
   InitializeMain();
   InitializeNotifier();
   InitializeCTREPDP();

hal/src/main/native/systemcore/HALInitializer.h

@@ -32,6 +32,7 @@ extern void InitializeEncoder();
 extern void InitializeFRCDriverStation();
 extern void InitializeHAL();
 extern void InitializeI2C();
+extern void InitializeIMU();
 extern void InitializeMain();
 extern void InitializeNotifier();
 extern void InitializeCTREPDP();

hal/src/main/native/systemcore/IMU.cpp

@@ -0,0 +1,130 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "hal/IMU.h"
+
+#include <numbers>
+
+#include <networktables/DoubleArrayTopic.h>
+#include <networktables/DoubleTopic.h>
+#include <networktables/NetworkTableInstance.h>
+
+#include "HALInitializer.h"
+#include "SystemServerInternal.h"
+#include "hal/Errors.h"
+
+#define IMU_PREFIX "/imu/"
+
+namespace {
+
+constexpr const char* kRawAccelKey = IMU_PREFIX "rawaccel";
+constexpr const char* kRawGyroKey = IMU_PREFIX "rawgyro";
+constexpr const char* kQuaternionKey = IMU_PREFIX "quat";
+constexpr const char* kEulerAngleKey = IMU_PREFIX "euler";
+constexpr const char* kYawFlatKey = IMU_PREFIX "yaw_flat";
+constexpr const char* kYawLandscapeKey = IMU_PREFIX "yaw_landscape";
+constexpr const char* kYawPortraitKey = IMU_PREFIX "yaw_portrait";
+
+struct IMU {
+  explicit IMU(nt::NetworkTableInstance inst)
+      : rawAccelSub{inst.GetDoubleArrayTopic(kRawAccelKey).Subscribe({})},
+        rawGyroSub{inst.GetDoubleArrayTopic(kRawGyroKey).Subscribe({})},
+        quatSub{inst.GetDoubleArrayTopic(kQuaternionKey).Subscribe({})},
+        eulerAngleSub{inst.GetDoubleArrayTopic(kEulerAngleKey).Subscribe({})},
+        yawFlatSub{inst.GetDoubleTopic(kYawFlatKey).Subscribe(0)},
+        yawLandscapeSub{inst.GetDoubleTopic(kYawLandscapeKey).Subscribe(0)},
+        yawPortraitSub{inst.GetDoubleTopic(kYawPortraitKey).Subscribe(0)} {}
+
+  nt::DoubleArraySubscriber rawAccelSub;
+  nt::DoubleArraySubscriber rawGyroSub;
+  nt::DoubleArraySubscriber quatSub;
+  nt::DoubleArraySubscriber eulerAngleSub;
+  nt::DoubleSubscriber yawFlatSub;
+  nt::DoubleSubscriber yawLandscapeSub;
+  nt::DoubleSubscriber yawPortraitSub;
+};
+
+static IMU* imu;
+
+constexpr double kDegreesToRadians = std::numbers::pi / 180.0;
+constexpr double kGToMetersPerSecondSquared = 9.80665;
+}  // namespace
+
+namespace hal::init {
+void InitializeIMU() {
+  static IMU imu_static{hal::GetSystemServer()};
+  imu = &imu_static;
+}
+}  // namespace hal::init
+
+extern "C" {
+void HAL_GetIMUAcceleration(HAL_Acceleration3d* accel, int32_t* status) {
+  auto update = imu->rawAccelSub.GetAtomic();
+  if (update.value.size() != 3) {
+    *status = INCOMPATIBLE_STATE;
+    return;
+  }
+  *accel =
+      HAL_Acceleration3d{.timestamp = update.time,
+                         .x = update.value[0] * kGToMetersPerSecondSquared,
+                         .y = update.value[1] * kGToMetersPerSecondSquared,
+                         .z = update.value[2] * kGToMetersPerSecondSquared};
+}
+
+void HAL_GetIMUGyroRates(HAL_GyroRate3d* rate, int32_t* status) {
+  auto update = imu->rawGyroSub.GetAtomic();
+  if (update.value.size() != 3) {
+    *status = INCOMPATIBLE_STATE;
+    return;
+  }
+
+  *rate = HAL_GyroRate3d{.timestamp = update.time,
+                         .x = update.value[0] * kDegreesToRadians,
+                         .y = update.value[1] * kDegreesToRadians,
+                         .z = update.value[2] * kDegreesToRadians};
+}
+
+void HAL_GetIMUEulerAngles(HAL_EulerAngles3d* angles, int32_t* status) {
+  auto update = imu->eulerAngleSub.GetAtomic();
+  if (update.value.size() != 3) {
+    *status = INCOMPATIBLE_STATE;
+    return;
+  }
+  *angles = HAL_EulerAngles3d{.timestamp = update.time,
+                              .x = update.value[0] * kDegreesToRadians,
+                              .y = update.value[1] * kDegreesToRadians,
+                              .z = update.value[2] * kDegreesToRadians};
+}
+
+void HAL_GetIMUQuaternion(HAL_Quaternion* quat, int32_t* status) {
+  auto update = imu->quatSub.GetAtomic();
+  if (update.value.size() != 4) {
+    *status = INCOMPATIBLE_STATE;
+    return;
+  }
+  *quat = HAL_Quaternion{.timestamp = update.time,
+                         .w = update.value[0],
+                         .x = update.value[1],
+                         .y = update.value[2],
+                         .z = update.value[3]};
+}
+
+double HAL_GetIMUYawFlat(int64_t* timestamp) {
+  auto update = imu->yawFlatSub.GetAtomic();
+  *timestamp = update.time;
+  return update.value * kDegreesToRadians;
+}
+
+double HAL_GetIMUYawLandscape(int64_t* timestamp) {
+  auto update = imu->yawLandscapeSub.GetAtomic();
+  *timestamp = update.time;
+  return update.value * kDegreesToRadians;
+}
+
+double HAL_GetIMUYawPortrait(int64_t* timestamp) {
+  auto update = imu->yawFlatSub.GetAtomic();
+  *timestamp = update.time;
+  return update.value * kDegreesToRadians;
+}
+}  // extern "C"

wpilibc/src/main/native/cpp/OnboardIMU.cpp

@@ -0,0 +1,131 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/OnboardIMU.h"
+
+#include <hal/IMU.h>
+
+#include "frc/Errors.h"
+
+using namespace frc;
+
+OnboardIMU::OnboardIMU(MountOrientation mountOrientation)
+    : m_mountOrientation{mountOrientation} {
+  // TODO: usage reporting
+}
+
+units::radian_t OnboardIMU::GetYawNoOffset() {
+  int64_t timestamp;
+  double val;
+  switch (m_mountOrientation) {
+    case kFlat:
+      val = HAL_GetIMUYawFlat(&timestamp);
+      break;
+    case kLandscape:
+      val = HAL_GetIMUYawLandscape(&timestamp);
+      break;
+    case kPortrait:
+      val = HAL_GetIMUYawPortrait(&timestamp);
+      break;
+    default:
+      val = 0;
+  }
+  return units::radian_t{val};
+}
+
+units::radian_t OnboardIMU::GetYaw() {
+  return GetYawNoOffset() - m_yawOffset;
+}
+
+void OnboardIMU::ResetYaw() {
+  m_yawOffset = GetYawNoOffset();
+}
+
+Rotation2d OnboardIMU::GetRotation2d() {
+  return Rotation2d{GetYaw()};
+}
+
+Rotation3d OnboardIMU::GetRotation3d() {
+  return Rotation3d{GetQuaternion()};
+}
+
+Quaternion OnboardIMU::GetQuaternion() {
+  HAL_Quaternion val;
+  int32_t status = 0;
+  HAL_GetIMUQuaternion(&val, &status);
+  FRC_CheckErrorStatus(status, "Onboard IMU");
+  return Quaternion{val.w, val.x, val.y, val.z};
+}
+
+units::radian_t GetAngleX() {
+  HAL_EulerAngles3d val;
+  int32_t status = 0;
+  HAL_GetIMUEulerAngles(&val, &status);
+  FRC_CheckErrorStatus(status, "Onboard IMU");
+  return units::radian_t{val.x};
+}
+
+units::radian_t GetAngleY() {
+  HAL_EulerAngles3d val;
+  int32_t status = 0;
+  HAL_GetIMUEulerAngles(&val, &status);
+  FRC_CheckErrorStatus(status, "Onboard IMU");
+  return units::radian_t{val.y};
+}
+
+units::radian_t GetAngleZ() {
+  HAL_EulerAngles3d val;
+  int32_t status = 0;
+  HAL_GetIMUEulerAngles(&val, &status);
+  FRC_CheckErrorStatus(status, "Onboard IMU");
+  return units::radian_t{val.z};
+}
+
+units::radians_per_second_t OnboardIMU::GetGyroRateX() {
+  HAL_GyroRate3d val;
+  int32_t status = 0;
+  HAL_GetIMUGyroRates(&val, &status);
+  FRC_CheckErrorStatus(status, "Onboard IMU");
+  return units::radians_per_second_t{val.x};
+}
+
+units::radians_per_second_t OnboardIMU::GetGyroRateY() {
+  HAL_GyroRate3d val;
+  int32_t status = 0;
+  HAL_GetIMUGyroRates(&val, &status);
+  FRC_CheckErrorStatus(status, "Onboard IMU");
+  return units::radians_per_second_t{val.y};
+}
+
+units::radians_per_second_t OnboardIMU::GetGyroRateZ() {
+  HAL_GyroRate3d val;
+  int32_t status = 0;
+  HAL_GetIMUGyroRates(&val, &status);
+  FRC_CheckErrorStatus(status, "Onboard IMU");
+  return units::radians_per_second_t{val.z};
+}
+
+units::meters_per_second_squared_t OnboardIMU::GetAccelX() {
+  HAL_Acceleration3d val;
+  int32_t status = 0;
+  HAL_GetIMUAcceleration(&val, &status);
+  FRC_CheckErrorStatus(status, "Onboard IMU");
+  return units::meters_per_second_squared_t{val.x};
+}
+
+units::meters_per_second_squared_t OnboardIMU::GetAccelY() {
+  HAL_Acceleration3d val;
+  int32_t status = 0;
+  HAL_GetIMUAcceleration(&val, &status);
+  FRC_CheckErrorStatus(status, "Onboard IMU");
+  return units::meters_per_second_squared_t{val.x};
+}
+
+units::meters_per_second_squared_t OnboardIMU::GetAccelZ() {
+  HAL_Acceleration3d val;
+  int32_t status = 0;
+  HAL_GetIMUAcceleration(&val, &status);
+  FRC_CheckErrorStatus(status, "Onboard IMU");
+  return units::meters_per_second_squared_t{val.x};
+}

wpilibc/src/main/native/include/frc/OnboardIMU.h

@@ -0,0 +1,129 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+#include <frc/geometry/Quaternion.h>
+#include <frc/geometry/Rotation2d.h>
+#include <frc/geometry/Rotation3d.h>
+
+#include <units/acceleration.h>
+#include <units/angle.h>
+#include <units/angular_velocity.h>
+
+namespace frc {
+
+/**
+ * SystemCore onboard IMU
+ */
+class OnboardIMU {
+ public:
+  /**
+   * A mount orientation of SystemCore
+   */
+  enum MountOrientation {
+    /** Flat */
+    kFlat,
+    /** Landscape */
+    kLandscape,
+    /** Portrait */
+    kPortrait
+  };
+
+  /**
+   * Constructs a handle to the SystemCore onboard IMU.
+   * @param mountOrientation the mount orientation of SystemCore to determine
+   * yaw.
+   */
+  explicit OnboardIMU(MountOrientation mountOrientation);
+
+  /**
+   * Get the yaw value
+   * @return yaw value
+   */
+  units::radian_t GetYaw();
+
+  /**
+   * Reset the current yaw value to 0. Future reads of the yaw value will be
+   * relative to the current orientation.
+   */
+  void ResetYaw();
+
+  /**
+   * Get the yaw as a Rotation2d.
+   * @return yaw
+   */
+  Rotation2d GetRotation2d();
+
+  /**
+   * Get the 3D orientation as a Rotation3d.
+   * @return 3D orientation
+   */
+  Rotation3d GetRotation3d();
+
+  /**
+   * Get the 3D orientation as a Quaternion.
+   * @return 3D orientation
+   */
+  Quaternion GetQuaternion();
+
+  /**
+   * Get the angle about the X axis of the IMU.
+   * @return angle about the X axis
+   */
+  units::radian_t GetAngleX();
+
+  /**
+   * Get the angle about the Y axis of the IMU.
+   * @return angle about the Y axis
+   */
+  units::radian_t GetAngleY();
+
+  /**
+   * Get the angle about the Z axis of the IMU.
+   * @return angle about the Z axis
+   */
+  units::radian_t GetAngleZ();
+
+  /**
+   * Get the angular rate about the X axis of the IMU.
+   * @return angular rate about the X axis
+   */
+  units::radians_per_second_t GetGyroRateX();
+
+  /**
+   * Get the angular rate about the Y axis of the IMU.
+   * @return angular rate about the Y axis
+   */
+  units::radians_per_second_t GetGyroRateY();
+
+  /**
+   * Get the angular rate about the Z axis of the IMU.
+   * @return angular rate about the Z axis
+   */
+  units::radians_per_second_t GetGyroRateZ();
+
+  /**
+   * Get the acceleration along the X axis of the IMU.
+   * @return acceleration along the X axis
+   */
+  units::meters_per_second_squared_t GetAccelX();
+
+  /**
+   * Get the acceleration along the Z axis of the IMU.
+   * @return acceleration along the Z axis
+   */
+  units::meters_per_second_squared_t GetAccelY();
+
+  /**
+   * Get the acceleration along the Z axis of the IMU.
+   * @return acceleration along the Z axis
+   */
+  units::meters_per_second_squared_t GetAccelZ();
+
+ private:
+  units::radian_t GetYawNoOffset();
+  MountOrientation m_mountOrientation;
+  units::radian_t m_yawOffset{0};
+};
+}  // namespace frc

wpilibj/src/main/java/edu/wpi/first/wpilibj/OnboardIMU.java

@@ -0,0 +1,193 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.IMUJNI;
+import edu.wpi.first.math.geometry.Quaternion;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+
+/** SystemCore onboard IMU. */
+public class OnboardIMU {
+  /** A mount orientation of SystemCore. */
+  public enum MountOrientation {
+    /** Flat. */
+    kFlat,
+    /** Landscape. */
+    kLandscape,
+    /** Portrait. */
+    kPortrait
+  }
+
+  /**
+   * Constructs a handle to the SystemCore onboard IMU.
+   *
+   * @param mountOrientation the mount orientation of SystemCore to determine yaw.
+   */
+  public OnboardIMU(MountOrientation mountOrientation) {
+    m_mountOrientation = mountOrientation;
+  }
+
+  /**
+   * Get the yaw value in radians.
+   *
+   * @return yaw value in radians
+   */
+  public double getYawRadians() {
+    return getYawNoOffset() - m_yawOffset;
+  }
+
+  /**
+   * Reset the current yaw value to 0. Future reads of the yaw value will be relative to the current
+   * orientation.
+   */
+  public void resetYaw() {
+    m_yawOffset = getYawNoOffset();
+  }
+
+  /**
+   * Get the yaw as a Rotation2d.
+   *
+   * @return yaw
+   */
+  public Rotation2d getRotation2d() {
+    return new Rotation2d(getYawRadians());
+  }
+
+  /**
+   * Get the 3D orientation as a Rotation3d.
+   *
+   * @return 3D orientation
+   */
+  public Rotation3d getRotation3d() {
+    return new Rotation3d(getQuaternion());
+  }
+
+  /**
+   * Get the 3D orientation as a Quaternion.
+   *
+   * @return 3D orientation
+   */
+  public Quaternion getQuaternion() {
+    double[] quatRaw = new double[4];
+    IMUJNI.getIMUQuaternion(quatRaw);
+    return new Quaternion(quatRaw[0], quatRaw[1], quatRaw[2], quatRaw[3]);
+  }
+
+  /**
+   * Get the angle about the X axis of the IMU in radians.
+   *
+   * @return angle about the X axis in radians
+   */
+  public double getAngleX() {
+    return getRawEulerAngles()[0];
+  }
+
+  /**
+   * Get the angle about the Y axis of the IMU in radians.
+   *
+   * @return angle about the Y axis in radians
+   */
+  public double getAngleY() {
+    return getRawEulerAngles()[1];
+  }
+
+  /**
+   * Get the angle about the Z axis of the IMU in radians.
+   *
+   * @return angle about the Z axis in radians
+   */
+  public double getAngleZ() {
+    return getRawEulerAngles()[2];
+  }
+
+  /**
+   * Get the angular rate about the X axis of the IMU in radians per second.
+   *
+   * @return angular rate about the X axis in radians per second
+   */
+  public double getGyroRateX() {
+    return getRawGyroRates()[0];
+  }
+
+  /**
+   * Get the angular rate about the Y axis of the IMU in radians per second.
+   *
+   * @return angular rate about the Y axis in radians per second
+   */
+  public double getGyroRateY() {
+    return getRawGyroRates()[1];
+  }
+
+  /**
+   * Get the angular rate about the Z axis of the IMU in radians per second.
+   *
+   * @return angular rate about the Z axis in radians per second
+   */
+  public double getGyroRateZ() {
+    return getRawGyroRates()[2];
+  }
+
+  /**
+   * Get the acceleration along the X axis of the IMU in meters per second squared.
+   *
+   * @return acceleration along the X axis in meters per second squared
+   */
+  public double getAccelX() {
+    return getRawAccels()[0];
+  }
+
+  /**
+   * Get the acceleration along the X axis of the IMU in meters per second squared.
+   *
+   * @return acceleration along the X axis in meters per second squared
+   */
+  public double getAccelY() {
+    return getRawAccels()[1];
+  }
+
+  /**
+   * Get the acceleration along the X axis of the IMU in meters per second squared.
+   *
+   * @return acceleration along the X axis in meters per second squared
+   */
+  public double getAccelZ() {
+    return getRawAccels()[2];
+  }
+
+  private double[] getRawEulerAngles() {
+    double[] anglesRaw = new double[3];
+    IMUJNI.getIMUEulerAngles(anglesRaw);
+    return anglesRaw;
+  }
+
+  private double[] getRawGyroRates() {
+    double[] ratesRaw = new double[3];
+    IMUJNI.getIMUGyroRates(ratesRaw);
+    return ratesRaw;
+  }
+
+  private double[] getRawAccels() {
+    double[] accelsRaw = new double[3];
+    IMUJNI.getIMUAcceleration(accelsRaw);
+    return accelsRaw;
+  }
+
+  private double getYawNoOffset() {
+    switch (m_mountOrientation) {
+      case kFlat:
+        return IMUJNI.getIMUYawFlat();
+      case kLandscape:
+        return IMUJNI.getIMUYawLandscape();
+      case kPortrait:
+        return IMUJNI.getIMUYawPortrait();
+      default:
+        return 0;
+    }
+  }
+
+  private final MountOrientation m_mountOrientation;
+  private double m_yawOffset;
+}