[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

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()