[wpilib][sim] Add Onboard IMU Sim (#8855)

This provides the ability to simulate parts of the Onboard IMU at the
HAL level. This allows team to use and simulate the IMU in code, and a
follow up PR could be made to the halsim_gui to add a new widget to view
and modify the data graphically.

Since the C++ IMU uses radians for angles that is what I did for the
simulator.

Partially deals with #8845

hal/src/main/java/org/wpilib/hardware/hal/simulation/IMUDataJNI.java

@@ -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.
+
+package org.wpilib.hardware.hal.simulation;
+
+import org.wpilib.hardware.hal.JNIWrapper;
+
+/** JNI for imu data. */
+public class IMUDataJNI extends JNIWrapper {
+  public static native void setAngleX(double angleRad);
+
+  public static native void setAngleY(double angleRad);
+
+  public static native void setAngleZ(double angleRad);
+
+  public static native void setGyroRateX(double rateRadPerSec);
+
+  public static native void setGyroRateY(double rateRadPerSec);
+
+  public static native void setGyroRateZ(double rateRadPerSec);
+
+  public static native void setAccelX(double accelMpss);
+
+  public static native void setAccelY(double accelMpss);
+
+  public static native void setAccelZ(double accelMpss);
+
+  public static native void setYaw(double angleRad);
+}

hal/src/main/native/cpp/jni/simulation/IMUDataJNI.cpp

@@ -0,0 +1,132 @@
+// 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 "org_wpilib_hardware_hal_simulation_IMUDataJNI.h"
+#include "wpi/hal/simulation/IMUData.h"
+
+extern "C" {
+
+/*
+ * Class:     org_wpilib_hardware_hal_simulation_IMUDataJNI
+ * Method:    setAngleX
+ * Signature: (D)V
+ */
+JNIEXPORT void JNICALL
+Java_org_wpilib_hardware_hal_simulation_IMUDataJNI_setAngleX
+  (JNIEnv*, jclass, jdouble angle)
+{
+  return HALSIM_SetIMUAngleX(angle);
+}
+
+/*
+ * Class:     org_wpilib_hardware_hal_simulation_IMUDataJNI
+ * Method:    setAngleY
+ * Signature: (D)V
+ */
+JNIEXPORT void JNICALL
+Java_org_wpilib_hardware_hal_simulation_IMUDataJNI_setAngleY
+  (JNIEnv*, jclass, jdouble angle)
+{
+  return HALSIM_SetIMUAngleY(angle);
+}
+
+/*
+ * Class:     org_wpilib_hardware_hal_simulation_IMUDataJNI
+ * Method:    setAngleZ
+ * Signature: (D)V
+ */
+JNIEXPORT void JNICALL
+Java_org_wpilib_hardware_hal_simulation_IMUDataJNI_setAngleZ
+  (JNIEnv*, jclass, jdouble angle)
+{
+  return HALSIM_SetIMUAngleZ(angle);
+}
+
+/*
+ * Class:     org_wpilib_hardware_hal_simulation_IMUDataJNI
+ * Method:    setGyroRateX
+ * Signature: (D)V
+ */
+JNIEXPORT void JNICALL
+Java_org_wpilib_hardware_hal_simulation_IMUDataJNI_setGyroRateX
+  (JNIEnv*, jclass, jdouble rate)
+{
+  return HALSIM_SetIMUGyroRateX(rate);
+}
+
+/*
+ * Class:     org_wpilib_hardware_hal_simulation_IMUDataJNI
+ * Method:    setGyroRateY
+ * Signature: (D)V
+ */
+JNIEXPORT void JNICALL
+Java_org_wpilib_hardware_hal_simulation_IMUDataJNI_setGyroRateY
+  (JNIEnv*, jclass, jdouble rate)
+{
+  return HALSIM_SetIMUGyroRateY(rate);
+}
+
+/*
+ * Class:     org_wpilib_hardware_hal_simulation_IMUDataJNI
+ * Method:    setGyroRateZ
+ * Signature: (D)V
+ */
+JNIEXPORT void JNICALL
+Java_org_wpilib_hardware_hal_simulation_IMUDataJNI_setGyroRateZ
+  (JNIEnv*, jclass, jdouble rate)
+{
+  return HALSIM_SetIMUGyroRateZ(rate);
+}
+
+/*
+ * Class:     org_wpilib_hardware_hal_simulation_IMUDataJNI
+ * Method:    setAccelX
+ * Signature: (D)V
+ */
+JNIEXPORT void JNICALL
+Java_org_wpilib_hardware_hal_simulation_IMUDataJNI_setAccelX
+  (JNIEnv*, jclass, jdouble accel)
+{
+  return HALSIM_SetIMUAccelX(accel);
+}
+
+/*
+ * Class:     org_wpilib_hardware_hal_simulation_IMUDataJNI
+ * Method:    setAccelY
+ * Signature: (D)V
+ */
+JNIEXPORT void JNICALL
+Java_org_wpilib_hardware_hal_simulation_IMUDataJNI_setAccelY
+  (JNIEnv*, jclass, jdouble accel)
+{
+  return HALSIM_SetIMUAccelY(accel);
+}
+
+/*
+ * Class:     org_wpilib_hardware_hal_simulation_IMUDataJNI
+ * Method:    setAccelZ
+ * Signature: (D)V
+ */
+JNIEXPORT void JNICALL
+Java_org_wpilib_hardware_hal_simulation_IMUDataJNI_setAccelZ
+  (JNIEnv*, jclass, jdouble accel)
+{
+  return HALSIM_SetIMUAccelZ(accel);
+}
+
+/*
+ * Class:     org_wpilib_hardware_hal_simulation_IMUDataJNI
+ * Method:    setYaw
+ * Signature: (D)V
+ */
+JNIEXPORT void JNICALL
+Java_org_wpilib_hardware_hal_simulation_IMUDataJNI_setYaw
+  (JNIEnv*, jclass, jdouble angle)
+{
+  return HALSIM_SetIMUYaw(angle);
+}
+
+}  // extern "C"

hal/src/main/native/include/wpi/hal/simulation/IMUData.h

@@ -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.
+
+#pragma once
+
+#include "wpi/hal/Types.h"
+#include "wpi/hal/simulation/NotifyListener.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_SetIMUAngleX(double angle);
+void HALSIM_SetIMUAngleY(double angle);
+void HALSIM_SetIMUAngleZ(double angle);
+
+void HALSIM_SetIMUGyroRateX(double rate);
+void HALSIM_SetIMUGyroRateY(double rate);
+void HALSIM_SetIMUGyroRateZ(double rate);
+
+void HALSIM_SetIMUAccelX(double accel);
+void HALSIM_SetIMUAccelY(double accel);
+void HALSIM_SetIMUAccelZ(double accel);
+
+void HALSIM_SetIMUYaw(double angle);
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif

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

@@ -74,6 +74,7 @@ void InitializeHAL() {
   InitializeDIOData();
   InitializeDriverStationData();
   InitializeEncoderData();
+  InitializeIMUData();
   InitializeI2CData();
   InitializeCTREPCMData();
   InitializeREVPHData();

hal/src/main/native/sim/HALInitializer.hpp

@@ -26,6 +26,7 @@ extern void InitializeDIOData();
 extern void InitializeDutyCycle();
 extern void InitializeDriverStationData();
 extern void InitializeEncoderData();
+extern void InitializeIMUData();
 extern void InitializeI2CData();
 extern void InitializeCTREPCMData();
 extern void InitializeREVPHData();

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

@@ -4,34 +4,62 @@
 
 #include "wpi/hal/IMU.h"
 
+#include "mockdata/IMUDataInternal.hpp"
+
+using namespace wpi::hal;
+
 extern "C" {
-// TODO(Ryan) implement sim
 void HAL_GetIMUAcceleration(HAL_Acceleration3d* accel, int32_t* status) {
-  *accel = {};
+  *accel = {
+      .timestamp = 0,
+      .x = SimIMUData->accelX,
+      .y = SimIMUData->accelY,
+      .z = SimIMUData->accelZ,
+  };
 }
 void HAL_GetIMUGyroRates(HAL_GyroRate3d* rate, int32_t* status) {
-  *rate = {};
+  *rate = {
+      .timestamp = 0,
+      .x = SimIMUData->gyroRateX,
+      .y = SimIMUData->gyroRateY,
+      .z = SimIMUData->gyroRateZ,
+  };
 }
 void HAL_GetIMUEulerAnglesFlat(HAL_EulerAngles3d* angles, int32_t* status) {
-  *angles = {};
+  *angles = {
+      .timestamp = 0,
+      .x = SimIMUData->angleX,
+      .y = SimIMUData->angleY,
+      .z = SimIMUData->angleZ,
+  };
 }
 void HAL_GetIMUEulerAnglesLandscape(HAL_EulerAngles3d* angles,
                                     int32_t* status) {
-  *angles = {};
+  *angles = {
+      .timestamp = 0,
+      .x = SimIMUData->angleX,
+      .y = SimIMUData->angleY,
+      .z = SimIMUData->angleZ,
+  };
 }
 void HAL_GetIMUEulerAnglesPortrait(HAL_EulerAngles3d* angles, int32_t* status) {
-  *angles = {};
+  *angles = {
+      .timestamp = 0,
+      .x = SimIMUData->angleX,
+      .y = SimIMUData->angleY,
+      .z = SimIMUData->angleZ,
+  };
 }
 void HAL_GetIMUQuaternion(HAL_Quaternion* quat, int32_t* status) {
   *quat = {};
 }
 double HAL_GetIMUYawFlat(int64_t* timestamp) {
-  return 0;
+  return SimIMUData->yaw;
 }
 double HAL_GetIMUYawLandscape(int64_t* timestamp) {
-  return 0;
+  return SimIMUData->yaw;
 }
 double HAL_GetIMUYawPortrait(int64_t* timestamp) {
-  return 0;
+  return SimIMUData->yaw;
 }
 }  // extern "C"

hal/src/main/native/sim/mockdata/IMUData.cpp

@@ -0,0 +1,39 @@
+// 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 "IMUDataInternal.hpp"
+
+using namespace wpi::hal;
+
+namespace wpi::hal {
+namespace init {
+void InitializeIMUData() {
+  static IMUData imu;
+  ::wpi::hal::SimIMUData = &imu;
+}
+}  // namespace init
+
+IMUData* SimIMUData;
+}  // namespace wpi::hal
+
+extern "C" {
+#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME)                      \
+  HAL_SIMDATAVALUE_DEFINE_CAPI_NOINDEX(TYPE, HALSIM, IMU##CAPINAME, \
+                                       SimIMUData, LOWERNAME)
+
+DEFINE_CAPI(double, AngleX, angleX)
+DEFINE_CAPI(double, AngleY, angleY)
+DEFINE_CAPI(double, AngleZ, angleZ)
+
+DEFINE_CAPI(double, GyroRateX, gyroRateX)
+DEFINE_CAPI(double, GyroRateY, gyroRateY)
+DEFINE_CAPI(double, GyroRateZ, gyroRateZ)
+
+DEFINE_CAPI(double, AccelX, accelX)
+DEFINE_CAPI(double, AccelY, accelY)
+DEFINE_CAPI(double, AccelZ, accelZ)
+
+DEFINE_CAPI(double, Yaw, yaw)
+
+}  // extern "C"

hal/src/main/native/sim/mockdata/IMUDataInternal.hpp

@@ -0,0 +1,43 @@
+// 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 "wpi/hal/simulation/SimDataValue.hpp"
+
+namespace wpi::hal {
+
+class IMUData {
+  HAL_SIMDATAVALUE_DEFINE_NAME(AngleX)
+  HAL_SIMDATAVALUE_DEFINE_NAME(AngleY)
+  HAL_SIMDATAVALUE_DEFINE_NAME(AngleZ)
+
+  HAL_SIMDATAVALUE_DEFINE_NAME(GyroRateX)
+  HAL_SIMDATAVALUE_DEFINE_NAME(GyroRateY)
+  HAL_SIMDATAVALUE_DEFINE_NAME(GyroRateZ)
+
+  HAL_SIMDATAVALUE_DEFINE_NAME(AccelX)
+  HAL_SIMDATAVALUE_DEFINE_NAME(AccelY)
+  HAL_SIMDATAVALUE_DEFINE_NAME(AccelZ)
+
+  HAL_SIMDATAVALUE_DEFINE_NAME(Yaw)
+
+ public:
+  SimDataValue<double, HAL_MakeDouble, GetAngleXName> angleX{0};
+  SimDataValue<double, HAL_MakeDouble, GetAngleYName> angleY{0};
+  SimDataValue<double, HAL_MakeDouble, GetAngleZName> angleZ{0};
+
+  SimDataValue<double, HAL_MakeDouble, GetGyroRateXName> gyroRateX{0};
+  SimDataValue<double, HAL_MakeDouble, GetGyroRateYName> gyroRateY{0};
+  SimDataValue<double, HAL_MakeDouble, GetGyroRateZName> gyroRateZ{0};
+
+  SimDataValue<double, HAL_MakeDouble, GetAccelXName> accelX{0};
+  SimDataValue<double, HAL_MakeDouble, GetAccelYName> accelY{0};
+  SimDataValue<double, HAL_MakeDouble, GetAccelZName> accelZ{0};
+
+  SimDataValue<double, HAL_MakeDouble, GetYawName> yaw{0};
+};
+extern IMUData* SimIMUData;
+
+}  // namespace wpi::hal

hal/src/main/native/systemcore/mockdata/IMUData.cpp

@@ -0,0 +1,25 @@
+// 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 "wpi/hal/simulation/SimDataValue.hpp"
+
+extern "C" {
+#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
+  HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, IMU##CAPINAME, RETURN)
+
+DEFINE_CAPI(double, AngleX, 0)
+DEFINE_CAPI(double, AngleY, 0)
+DEFINE_CAPI(double, AngleZ, 0)
+
+DEFINE_CAPI(double, GyroRateX, 0)
+DEFINE_CAPI(double, GyroRateY, 0)
+DEFINE_CAPI(double, GyroRateZ, 0)
+
+DEFINE_CAPI(double, AccelX, 0)
+DEFINE_CAPI(double, AccelY, 0)
+DEFINE_CAPI(double, AccelZ, 0)
+
+DEFINE_CAPI(double, Yaw, 0)
+
+}  // extern "C"

wpilibc/robotpy_pybind_build_info.bzl

@@ -1441,6 +1441,16 @@ def wpilib_simulation_extension(srcs = [], header_to_dat_deps = [], extra_hdrs =
                 ("wpi::sim::NiDsPS5ControllerSim", "wpi__sim__NiDsPS5ControllerSim.hpp"),
             ],
         ),
+        struct(
+            class_name = "OnboardIMUSim",
+            yml_file = "semiwrap/simulation/OnboardIMUSim.yml",
+            header_root = "$(execpath :robotpy-native-wpilib.copy_headers)",
+            header_file = "$(execpath :robotpy-native-wpilib.copy_headers)/wpi/simulation/OnboardIMUSim.hpp",
+            tmpl_class_names = [],
+            trampolines = [
+                ("wpi::sim::OnboardIMUSim", "wpi__sim__OnboardIMUSim.hpp"),
+            ],
+        ),
         struct(
             class_name = "PWMSim",
             yml_file = "semiwrap/simulation/PWMSim.yml",

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

@@ -148,7 +148,7 @@ wpi::units::meters_per_second_squared_t OnboardIMU::GetAccelY() {
   int32_t status = 0;
   HAL_GetIMUAcceleration(&val, &status);
   WPILIB_CheckErrorStatus(status, "Onboard IMU");
-  return wpi::units::meters_per_second_squared_t{val.x};
+  return wpi::units::meters_per_second_squared_t{val.y};
 }
 
 wpi::units::meters_per_second_squared_t OnboardIMU::GetAccelZ() {
@@ -156,5 +156,5 @@ wpi::units::meters_per_second_squared_t OnboardIMU::GetAccelZ() {
   int32_t status = 0;
   HAL_GetIMUAcceleration(&val, &status);
   WPILIB_CheckErrorStatus(status, "Onboard IMU");
-  return wpi::units::meters_per_second_squared_t{val.x};
+  return wpi::units::meters_per_second_squared_t{val.z};
 }

wpilibc/src/main/native/cpp/simulation/OnboardIMUSim.cpp

@@ -0,0 +1,49 @@
+// 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 "wpi/simulation/OnboardIMUSim.hpp"
+
+#include "wpi/hal/simulation/IMUData.h"
+
+namespace wpi::sim {
+
+void OnboardIMUSim::SetAngleX(wpi::units::radian_t angle) {
+  HALSIM_SetIMUAngleX(angle.to<double>());
+}
+void OnboardIMUSim::SetAngleY(wpi::units::radian_t angle) {
+  HALSIM_SetIMUAngleY(angle.to<double>());
+}
+void OnboardIMUSim::SetAngleZ(wpi::units::radian_t angle) {
+  HALSIM_SetIMUAngleZ(angle.to<double>());
+}
+
+void OnboardIMUSim::SetGyroRateX(wpi::units::radians_per_second_t rate) {
+  HALSIM_SetIMUGyroRateX(rate.to<double>());
+}
+
+void OnboardIMUSim::SetGyroRateY(wpi::units::radians_per_second_t rate) {
+  HALSIM_SetIMUGyroRateY(rate.to<double>());
+}
+
+void OnboardIMUSim::SetGyroRateZ(wpi::units::radians_per_second_t rate) {
+  HALSIM_SetIMUGyroRateZ(rate.to<double>());
+}
+
+void OnboardIMUSim::SetAccelX(wpi::units::meters_per_second_squared_t accel) {
+  HALSIM_SetIMUAccelX(accel.to<double>());
+}
+
+void OnboardIMUSim::SetAccelY(wpi::units::meters_per_second_squared_t accel) {
+  HALSIM_SetIMUAccelY(accel.to<double>());
+}
+
+void OnboardIMUSim::SetAccelZ(wpi::units::meters_per_second_squared_t accel) {
+  HALSIM_SetIMUAccelZ(accel.to<double>());
+}
+
+void OnboardIMUSim::SetYaw(wpi::units::radian_t angle) {
+  HALSIM_SetIMUYaw(angle.to<double>());
+}
+
+}  // namespace wpi::sim

wpilibc/src/main/native/include/wpi/simulation/OnboardIMUSim.hpp

@@ -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.
+
+#pragma once
+
+#include "wpi/units/acceleration.hpp"
+#include "wpi/units/angle.hpp"
+#include "wpi/units/angular_velocity.hpp"
+
+namespace wpi::sim {
+
+class OnboardIMUSim {
+ public:
+  void SetAngleX(wpi::units::radian_t angle);
+  void SetAngleY(wpi::units::radian_t angle);
+  void SetAngleZ(wpi::units::radian_t angle);
+
+  void SetGyroRateX(wpi::units::radians_per_second_t rate);
+  void SetGyroRateY(wpi::units::radians_per_second_t rate);
+  void SetGyroRateZ(wpi::units::radians_per_second_t rate);
+
+  void SetAccelX(wpi::units::meters_per_second_squared_t accel);
+  void SetAccelY(wpi::units::meters_per_second_squared_t accel);
+  void SetAccelZ(wpi::units::meters_per_second_squared_t accel);
+
+  void SetYaw(wpi::units::radian_t angle);
+};
+
+}  // namespace wpi::sim

wpilibc/src/main/python/pyproject.toml

@@ -276,6 +276,7 @@ JoystickSim = "wpi/simulation/JoystickSim.hpp"
 LinearSystemSim = "wpi/simulation/LinearSystemSim.hpp"
 NiDsPS4ControllerSim = "wpi/simulation/NiDsPS4ControllerSim.hpp"
 NiDsPS5ControllerSim = "wpi/simulation/NiDsPS5ControllerSim.hpp"
+OnboardIMUSim = "wpi/simulation/OnboardIMUSim.hpp"
 PWMSim = "wpi/simulation/PWMSim.hpp"
 PneumaticsBaseSim = "wpi/simulation/PneumaticsBaseSim.hpp"
 PowerDistributionSim = "wpi/simulation/PowerDistributionSim.hpp"

wpilibc/src/main/python/semiwrap/simulation/OnboardIMUSim.yml

@@ -0,0 +1,13 @@
+classes:
+  wpi::sim::OnboardIMUSim:
+    methods:
+      SetAngleX:
+      SetAngleY:
+      SetAngleZ:
+      SetGyroRateX:
+      SetGyroRateY:
+      SetGyroRateZ:
+      SetAccelX:
+      SetAccelY:
+      SetAccelZ:
+      SetYaw:

wpilibc/src/main/python/wpilib/simulation/__init__.py

@@ -34,6 +34,7 @@ from ._simulation import (
     NiDsPS5ControllerSim,
     NiDsStadiaControllerSim,
     NiDsXboxControllerSim,
+    OnboardIMUSim,
     OpModeOptions,
     PWMMotorControllerSim,
     PWMSim,
@@ -92,6 +93,7 @@ __all__ = [
     "NiDsPS5ControllerSim",
     "NiDsStadiaControllerSim",
     "NiDsXboxControllerSim",
+    "OnboardIMUSim",
     "OpModeOptions",
     "PWMMotorControllerSim",
     "PWMSim",

wpilibc/src/test/native/cpp/OnboardIMUTest.cpp

@@ -0,0 +1,59 @@
+// 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 "wpi/hardware/imu/OnboardIMU.hpp"
+
+#include <gtest/gtest.h>
+
+#include "wpi/simulation/OnboardIMUSim.hpp"
+
+using namespace wpi;
+
+TEST(OnboardIMUTest, SimDevices) {
+  OnboardIMU imu{OnboardIMU::FLAT};
+
+  EXPECT_EQ(0.0, imu.GetAngleX().value());
+  EXPECT_EQ(0.0, imu.GetAngleY().value());
+  EXPECT_EQ(0.0, imu.GetAngleZ().value());
+
+  EXPECT_EQ(0.0, imu.GetGyroRateX().value());
+  EXPECT_EQ(0.0, imu.GetGyroRateY().value());
+  EXPECT_EQ(0.0, imu.GetGyroRateZ().value());
+
+  EXPECT_EQ(0.0, imu.GetAccelX().value());
+  EXPECT_EQ(0.0, imu.GetAccelY().value());
+  EXPECT_EQ(0.0, imu.GetAccelZ().value());
+
+  EXPECT_EQ(0.0, imu.GetYaw().value());
+
+  sim::OnboardIMUSim sim{};
+
+  sim.SetAngleX(wpi::units::radian_t{1});
+  sim.SetAngleY(wpi::units::radian_t{2});
+  sim.SetAngleZ(wpi::units::radian_t{3});
+
+  sim.SetGyroRateX(wpi::units::radians_per_second_t{3.504});
+  sim.SetGyroRateY(wpi::units::radians_per_second_t{1.91});
+  sim.SetGyroRateZ(wpi::units::radians_per_second_t{22.9});
+
+  sim.SetAccelX(wpi::units::meters_per_second_squared_t{-1});
+  sim.SetAccelY(wpi::units::meters_per_second_squared_t{-2});
+  sim.SetAccelZ(wpi::units::meters_per_second_squared_t{-3});
+
+  sim.SetYaw(wpi::units::radian_t{1.234});
+
+  EXPECT_EQ(1.0, imu.GetAngleX().value());
+  EXPECT_EQ(2.0, imu.GetAngleY().value());
+  EXPECT_EQ(3.0, imu.GetAngleZ().value());
+
+  EXPECT_EQ(3.504, imu.GetGyroRateX().value());
+  EXPECT_EQ(1.91, imu.GetGyroRateY().value());
+  EXPECT_EQ(22.9, imu.GetGyroRateZ().value());
+
+  EXPECT_EQ(-1.0, imu.GetAccelX().value());
+  EXPECT_EQ(-2.0, imu.GetAccelY().value());
+  EXPECT_EQ(-3.0, imu.GetAccelZ().value());
+
+  EXPECT_EQ(1.234, imu.GetYaw().value());
+}

wpilibc/src/test/python/test_onboard_imu.py

@@ -0,0 +1,41 @@
+from wpilib import OnboardIMU
+from wpilib.simulation import OnboardIMUSim
+
+def test_sim_device() -> None:
+
+    imu = OnboardIMU(OnboardIMU.MountOrientation.FLAT)
+    sim = OnboardIMUSim()
+
+    assert 0.0 == imu.getAngleX()
+    assert 0.0 == imu.getAngleY()
+    assert 0.0 == imu.getAngleZ()
+    assert 0.0 == imu.getGyroRateX()
+    assert 0.0 == imu.getGyroRateY()
+    assert 0.0 == imu.getGyroRateZ()
+    assert 0.0 == imu.getAccelX()
+    assert 0.0 == imu.getAccelY()
+    assert 0.0 == imu.getAccelZ()
+    
+    sim.setAngleX(1)
+    sim.setAngleY(2)
+    sim.setAngleZ(3)
+    
+    sim.setGyroRateX(3.504)
+    sim.setGyroRateY(1.91)
+    sim.setGyroRateZ(22.9)
+    
+    sim.setAccelX(-1)
+    sim.setAccelY(-2)
+    sim.setAccelZ(-3)
+
+    assert 1.0 == imu.getAngleX()
+    assert 2.0 == imu.getAngleY()
+    assert 3.0 == imu.getAngleZ()
+
+    assert 3.504 == imu.getGyroRateX()
+    assert 1.91 == imu.getGyroRateY()
+    assert 22.9 == imu.getGyroRateZ()
+
+    assert -1.0 == imu.getAccelX()
+    assert -2.0 == imu.getAccelY()
+    assert -3.0 == imu.getAccelZ()

wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Drivetrain.cpp

@@ -132,8 +132,7 @@ void Drivetrain::SimulationPeriodic() {
   leftEncoderSim.SetRate(drivetrainSimulator.GetLeftVelocity().value());
   rightEncoderSim.SetDistance(drivetrainSimulator.GetRightPosition().value());
   rightEncoderSim.SetRate(drivetrainSimulator.GetRightVelocity().value());
-  // gyroSim.SetAngle(-drivetrainSimulator.GetHeading().Degrees().value());
-  // // TODO(Ryan): fixup when sim implemented
+  imuSim.SetYaw(-drivetrainSimulator.GetHeading().Degrees());
 }
 
 void Drivetrain::Periodic() {

wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.hpp

@@ -23,6 +23,7 @@
 #include "wpi/nt/NetworkTableInstance.hpp"
 #include "wpi/simulation/DifferentialDrivetrainSim.hpp"
 #include "wpi/simulation/EncoderSim.hpp"
+#include "wpi/simulation/OnboardIMUSim.hpp"
 #include "wpi/smartdashboard/Field2d.hpp"
 #include "wpi/units/angle.hpp"
 #include "wpi/units/angular_velocity.hpp"
@@ -166,6 +167,7 @@ class Drivetrain {
   // Simulation classes
   wpi::sim::EncoderSim leftEncoderSim{leftEncoder};
   wpi::sim::EncoderSim rightEncoderSim{rightEncoder};
+  wpi::sim::OnboardIMUSim imuSim;
   wpi::Field2d fieldSim;
   wpi::Field2d fieldApproximation;
   wpi::math::LinearSystem<2, 2, 2> drivetrainSystem =

wpilibj/src/main/java/org/wpilib/simulation/OnboardIMUSim.java

@@ -0,0 +1,104 @@
+// 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 org.wpilib.simulation;
+
+import org.wpilib.hardware.hal.simulation.IMUDataJNI;
+
+/** Class to control a simulated onboard IMU. */
+public final class OnboardIMUSim {
+  private OnboardIMUSim() {
+    throw new UnsupportedOperationException("This is a utility class!");
+  }
+
+  /**
+   * Change the x-angle.
+   *
+   * @param angleRad the new angle
+   */
+  public static void setAngleX(double angleRad) {
+    IMUDataJNI.setAngleX(angleRad);
+  }
+
+  /**
+   * Change the y-angle.
+   *
+   * @param angleRad the new angle
+   */
+  public static void setAngleY(double angleRad) {
+    IMUDataJNI.setAngleY(angleRad);
+  }
+
+  /**
+   * Change the z-angle.
+   *
+   * @param angleRad the new angle
+   */
+  public static void setAngleZ(double angleRad) {
+    IMUDataJNI.setAngleZ(angleRad);
+  }
+
+  /**
+   * Change the x-gyro rate.
+   *
+   * @param rateRadPerSec the new rate
+   */
+  public static void setGyroRateX(double rateRadPerSec) {
+    IMUDataJNI.setGyroRateX(rateRadPerSec);
+  }
+
+  /**
+   * Change the y-gyro rate.
+   *
+   * @param rateRadPerSec the new rate
+   */
+  public static void setGyroRateY(double rateRadPerSec) {
+    IMUDataJNI.setGyroRateY(rateRadPerSec);
+  }
+
+  /**
+   * Change the z-gyro rate.
+   *
+   * @param rateRadPerSec the new rate
+   */
+  public static void setGyroRateZ(double rateRadPerSec) {
+    IMUDataJNI.setGyroRateZ(rateRadPerSec);
+  }
+
+  /**
+   * Change the x-accel rate.
+   *
+   * @param accelMpss the new acceleration
+   */
+  public static void setAccelX(double accelMpss) {
+    IMUDataJNI.setAccelX(accelMpss);
+  }
+
+  /**
+   * Change the y-accel rate.
+   *
+   * @param accelMpss the new acceleration
+   */
+  public static void setAccelY(double accelMpss) {
+    IMUDataJNI.setAccelY(accelMpss);
+  }
+
+  /**
+   * Change the z-accel rate.
+   *
+   * @param accelMpss the new acceleration
+   */
+  public static void setAccelZ(double accelMpss) {
+    IMUDataJNI.setAccelZ(accelMpss);
+  }
+
+  /**
+   * Change the yaw.
+   *
+   * @param angleRad the new yaw angle
+   */
+  public static void setYaw(double angleRad) {
+    IMUDataJNI.setYaw(angleRad);
+  }
+}

wpilibj/src/test/java/org/wpilib/hardware/imu/OnboardIMUTest.java

@@ -0,0 +1,59 @@
+// 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 org.wpilib.hardware.imu;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.Test;
+import org.wpilib.simulation.OnboardIMUSim;
+
+class OnboardIMUTest {
+  @Test
+  void testOnboardIMU() {
+    OnboardIMU imu = new OnboardIMU(OnboardIMU.MountOrientation.FLAT);
+
+    assertEquals(0.0, imu.getAngleX());
+    assertEquals(0.0, imu.getAngleY());
+    assertEquals(0.0, imu.getAngleZ());
+
+    assertEquals(0.0, imu.getGyroRateX());
+    assertEquals(0.0, imu.getGyroRateY());
+    assertEquals(0.0, imu.getGyroRateZ());
+
+    assertEquals(0.0, imu.getAccelX());
+    assertEquals(0.0, imu.getAccelY());
+    assertEquals(0.0, imu.getAccelZ());
+
+    assertEquals(0.0, imu.getYawRadians());
+
+    OnboardIMUSim.setAngleX(1);
+    OnboardIMUSim.setAngleY(2);
+    OnboardIMUSim.setAngleZ(3);
+
+    OnboardIMUSim.setGyroRateX(3.504);
+    OnboardIMUSim.setGyroRateY(1.91);
+    OnboardIMUSim.setGyroRateZ(22.9);
+
+    OnboardIMUSim.setAccelX(-1);
+    OnboardIMUSim.setAccelY(-2);
+    OnboardIMUSim.setAccelZ(-3);
+
+    OnboardIMUSim.setYaw(1.234);
+
+    assertEquals(1.0, imu.getAngleX());
+    assertEquals(2.0, imu.getAngleY());
+    assertEquals(3.0, imu.getAngleZ());
+
+    assertEquals(3.504, imu.getGyroRateX());
+    assertEquals(1.91, imu.getGyroRateY());
+    assertEquals(22.9, imu.getGyroRateZ());
+
+    assertEquals(-1.0, imu.getAccelX());
+    assertEquals(-2.0, imu.getAccelY());
+    assertEquals(-3.0, imu.getAccelZ());
+
+    assertEquals(1.234, imu.getYawRadians());
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/differentialdriveposeestimator/Drivetrain.java

@@ -30,6 +30,7 @@ import org.wpilib.networktables.DoubleArrayEntry;
 import org.wpilib.networktables.DoubleArrayTopic;
 import org.wpilib.simulation.DifferentialDrivetrainSim;
 import org.wpilib.simulation.EncoderSim;
+import org.wpilib.simulation.OnboardIMUSim;
 import org.wpilib.smartdashboard.Field2d;
 import org.wpilib.smartdashboard.SmartDashboard;
 import org.wpilib.system.RobotController;
@@ -253,8 +254,7 @@ public class Drivetrain {
     leftEncoderSim.setRate(drivetrainSimulator.getLeftVelocity());
     rightEncoderSim.setDistance(drivetrainSimulator.getRightPosition());
     rightEncoderSim.setRate(drivetrainSimulator.getRightVelocity());
-    // gyroSim.setAngle(-drivetrainSimulator.getHeading().getDegrees()); // TODO(Ryan): fixup
-    // when sim implemented
+    OnboardIMUSim.setYaw(-drivetrainSimulator.getHeading().getRadians());
   }
 
   /** This function is called periodically, no matter the mode. */