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