[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"