[wpilib] Make ADIS IMU classes unit-safe (#3860)

The gyro rate getters were removed since that data isn't available.
2026-06-29 02:21:44 +00:00 · 2022-01-03 20:00:53 -08:00
parent 947f589916
commit a2510aaa0e
12 changed files with 381 additions and 666 deletions
--- a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp
+++ b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp
@@ -66,10 +66,11 @@ inline void ADISReportError(int32_t status, const char* file, int line,
 #define REPORT_ERROR(msg) \
  ADISReportError(err::Error, __FILE__, __LINE__, __FUNCTION__, "{}", msg)

-ADIS16448_IMU::ADIS16448_IMU() : ADIS16448_IMU(kZ, SPI::Port::kMXP, 4) {}
+ADIS16448_IMU::ADIS16448_IMU()
+    : ADIS16448_IMU(kZ, SPI::Port::kMXP, CalibrationTime::_512ms) {}

 ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port,
-                             uint16_t cal_time)
+                             CalibrationTime cal_time)
    : m_yaw_axis(yaw_axis),
      m_spi_port(port),
      m_simDevice("Gyro:ADIS16448", port) {
@@ -80,12 +81,6 @@ ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port,
        m_simDevice.CreateDouble("gyro_angle_y", hal::SimDevice::kInput, 0.0);
    m_simGyroAngleZ =
        m_simDevice.CreateDouble("gyro_angle_z", hal::SimDevice::kInput, 0.0);
-    m_simGyroRateX =
-        m_simDevice.CreateDouble("gyro_rate_x", hal::SimDevice::kInput, 0.0);
-    m_simGyroRateY =
-        m_simDevice.CreateDouble("gyro_rate_y", hal::SimDevice::kInput, 0.0);
-    m_simGyroRateZ =
-        m_simDevice.CreateDouble("gyro_rate_z", hal::SimDevice::kInput, 0.0);
    m_simAccelX =
        m_simDevice.CreateDouble("accel_x", hal::SimDevice::kInput, 0.0);
    m_simAccelY =
@@ -311,12 +306,12 @@ bool ADIS16448_IMU::SwitchToAutoSPI() {
 /**
 *
 **/
-int ADIS16448_IMU::ConfigCalTime(int new_cal_time) {
+int ADIS16448_IMU::ConfigCalTime(CalibrationTime new_cal_time) {
  if (m_calibration_time == new_cal_time) {
    return 1;
  } else {
-    m_calibration_time = static_cast<uint16_t>(new_cal_time);
-    m_avg_size = m_calibration_time * 819;
+    m_calibration_time = new_cal_time;
+    m_avg_size = static_cast<uint16_t>(m_calibration_time) * 819;
    InitOffsetBuffer(m_avg_size);
    return 0;
  }
@@ -348,16 +343,6 @@ void ADIS16448_IMU::Calibrate() {
  // m_gyro_offset_z << std::endl;
 }

-int ADIS16448_IMU::SetYawAxis(IMUAxis yaw_axis) {
-  if (m_yaw_axis == yaw_axis) {
-    return 1;
-  } else {
-    m_yaw_axis = yaw_axis;
-    Reset();
-    return 0;
-  }
-}
-
 /**
 * This function reads the contents of an 8-bit register location by
 *transmitting the register location byte along with a null (0x00) byte using
@@ -744,12 +729,7 @@ int ADIS16448_IMU::ConfigDecRate(uint16_t DecimationSetting) {
  return 0;
 }

-/**
- * This function returns the most recent integrated angle for the axis chosen by
- *m_yaw_axis. This function is most useful in situations where the yaw axis may
- *not coincide with the IMU Z axis.
- **/
-double ADIS16448_IMU::GetAngle() const {
+units::degree_t ADIS16448_IMU::GetAngle() const {
  switch (m_yaw_axis) {
    case kX:
      return GetGyroAngleX();
@@ -758,142 +738,115 @@ double ADIS16448_IMU::GetAngle() const {
    case kZ:
      return GetGyroAngleZ();
    default:
-      return 0.0;
+      return 0_deg;
  }
 }

-double ADIS16448_IMU::GetRate() const {
-  switch (m_yaw_axis) {
-    case kX:
-      return GetGyroRateX();
-    case kY:
-      return GetGyroRateY();
-    case kZ:
-      return GetGyroRateZ();
-    default:
-      return 0.0;
+units::degree_t ADIS16448_IMU::GetGyroAngleX() const {
+  if (m_simGyroAngleX) {
+    return units::degree_t{m_simGyroAngleX.Get()};
  }
+  std::scoped_lock sync(m_mutex);
+  return units::degree_t{m_integ_gyro_x};
+}
+
+units::degree_t ADIS16448_IMU::GetGyroAngleY() const {
+  if (m_simGyroAngleY) {
+    return units::degree_t{m_simGyroAngleY.Get()};
+  }
+  std::scoped_lock sync(m_mutex);
+  return units::degree_t{m_integ_gyro_y};
+}
+
+units::degree_t ADIS16448_IMU::GetGyroAngleZ() const {
+  if (m_simGyroAngleZ) {
+    return units::degree_t{m_simGyroAngleZ.Get()};
+  }
+  std::scoped_lock sync(m_mutex);
+  return units::degree_t{m_integ_gyro_z};
+}
+
+units::meters_per_second_squared_t ADIS16448_IMU::GetAccelX() const {
+  if (m_simAccelX) {
+    return units::meters_per_second_squared_t{m_simAccelX.Get()};
+  }
+  std::scoped_lock sync(m_mutex);
+  return m_accel_x * 9.81_mps_sq;
+}
+
+units::meters_per_second_squared_t ADIS16448_IMU::GetAccelY() const {
+  if (m_simAccelY) {
+    return units::meters_per_second_squared_t{m_simAccelY.Get()};
+  }
+  std::scoped_lock sync(m_mutex);
+  return m_accel_y * 9.81_mps_sq;
+}
+
+units::meters_per_second_squared_t ADIS16448_IMU::GetAccelZ() const {
+  if (m_simAccelZ) {
+    return units::meters_per_second_squared_t{m_simAccelZ.Get()};
+  }
+  std::scoped_lock sync(m_mutex);
+  return m_accel_z * 9.81_mps_sq;
+}
+
+units::tesla_t ADIS16448_IMU::GetMagneticFieldX() const {
+  std::scoped_lock sync(m_mutex);
+  return units::gauss_t{m_mag_x * 1e-3};
+}
+
+units::tesla_t ADIS16448_IMU::GetMagneticFieldY() const {
+  std::scoped_lock sync(m_mutex);
+  return units::gauss_t{m_mag_y * 1e-3};
+}
+
+units::tesla_t ADIS16448_IMU::GetMagneticFieldZ() const {
+  std::scoped_lock sync(m_mutex);
+  return units::gauss_t{m_mag_z * 1e-3};
+}
+
+units::degree_t ADIS16448_IMU::GetXComplementaryAngle() const {
+  std::scoped_lock sync(m_mutex);
+  return units::degree_t{m_compAngleX};
+}
+
+units::degree_t ADIS16448_IMU::GetYComplementaryAngle() const {
+  std::scoped_lock sync(m_mutex);
+  return units::degree_t{m_compAngleY};
+}
+
+units::degree_t ADIS16448_IMU::GetXFilteredAccelAngle() const {
+  std::scoped_lock sync(m_mutex);
+  return units::degree_t{m_accelAngleX};
+}
+
+units::degree_t ADIS16448_IMU::GetYFilteredAccelAngle() const {
+  std::scoped_lock sync(m_mutex);
+  return units::degree_t{m_accelAngleY};
+}
+
+units::pounds_per_square_inch_t ADIS16448_IMU::GetBarometricPressure() const {
+  std::scoped_lock sync(m_mutex);
+  return units::mbar_t{m_baro};
+}
+
+units::celsius_t ADIS16448_IMU::GetTemperature() const {
+  std::scoped_lock sync(m_mutex);
+  return units::celsius_t{m_temp};
 }

 ADIS16448_IMU::IMUAxis ADIS16448_IMU::GetYawAxis() const {
  return m_yaw_axis;
 }

-double ADIS16448_IMU::GetGyroAngleX() const {
-  if (m_simGyroAngleX) {
-    return m_simGyroAngleX.Get();
+int ADIS16448_IMU::SetYawAxis(IMUAxis yaw_axis) {
+  if (m_yaw_axis == yaw_axis) {
+    return 1;
+  } else {
+    m_yaw_axis = yaw_axis;
+    Reset();
+    return 0;
  }
-  std::scoped_lock sync(m_mutex);
-  return m_integ_gyro_x;
-}
-
-double ADIS16448_IMU::GetGyroAngleY() const {
-  if (m_simGyroAngleY) {
-    return m_simGyroAngleY.Get();
-  }
-  std::scoped_lock sync(m_mutex);
-  return m_integ_gyro_y;
-}
-
-double ADIS16448_IMU::GetGyroAngleZ() const {
-  if (m_simGyroAngleZ) {
-    return m_simGyroAngleZ.Get();
-  }
-  std::scoped_lock sync(m_mutex);
-  return m_integ_gyro_z;
-}
-
-double ADIS16448_IMU::GetGyroRateX() const {
-  if (m_simGyroRateX) {
-    return m_simGyroRateX.Get();
-  }
-  std::scoped_lock sync(m_mutex);
-  return m_gyro_x;
-}
-
-double ADIS16448_IMU::GetGyroRateY() const {
-  if (m_simGyroRateY) {
-    return m_simGyroRateY.Get();
-  }
-  std::scoped_lock sync(m_mutex);
-  return m_gyro_y;
-}
-
-double ADIS16448_IMU::GetGyroRateZ() const {
-  if (m_simGyroRateZ) {
-    return m_simGyroRateZ.Get();
-  }
-  std::scoped_lock sync(m_mutex);
-  return m_gyro_z;
-}
-
-double ADIS16448_IMU::GetAccelX() const {
-  if (m_simAccelX) {
-    return m_simAccelX.Get();
-  }
-  std::scoped_lock sync(m_mutex);
-  return m_accel_x;
-}
-
-double ADIS16448_IMU::GetAccelY() const {
-  if (m_simAccelY) {
-    return m_simAccelY.Get();
-  }
-  std::scoped_lock sync(m_mutex);
-  return m_accel_y;
-}
-
-double ADIS16448_IMU::GetAccelZ() const {
-  if (m_simAccelZ) {
-    return m_simAccelZ.Get();
-  }
-  std::scoped_lock sync(m_mutex);
-  return m_accel_z;
-}
-
-double ADIS16448_IMU::GetMagInstantX() const {
-  std::scoped_lock sync(m_mutex);
-  return m_mag_x;
-}
-
-double ADIS16448_IMU::GetMagInstantY() const {
-  std::scoped_lock sync(m_mutex);
-  return m_mag_y;
-}
-
-double ADIS16448_IMU::GetMagInstantZ() const {
-  std::scoped_lock sync(m_mutex);
-  return m_mag_z;
-}
-
-double ADIS16448_IMU::GetXComplementaryAngle() const {
-  std::scoped_lock sync(m_mutex);
-  return m_compAngleX;
-}
-
-double ADIS16448_IMU::GetYComplementaryAngle() const {
-  std::scoped_lock sync(m_mutex);
-  return m_compAngleY;
-}
-
-double ADIS16448_IMU::GetXFilteredAccelAngle() const {
-  std::scoped_lock sync(m_mutex);
-  return m_accelAngleX;
-}
-
-double ADIS16448_IMU::GetYFilteredAccelAngle() const {
-  std::scoped_lock sync(m_mutex);
-  return m_accelAngleY;
-}
-
-double ADIS16448_IMU::GetBarometricPressure() const {
-  std::scoped_lock sync(m_mutex);
-  return m_baro;
-}
-
-double ADIS16448_IMU::GetTemperature() const {
-  std::scoped_lock sync(m_mutex);
-  return m_temp;
 }

 int ADIS16448_IMU::GetPort() const {
@@ -909,6 +862,7 @@ int ADIS16448_IMU::GetPort() const {
 void ADIS16448_IMU::InitSendable(nt::NTSendableBuilder& builder) {
  builder.SetSmartDashboardType("ADIS16448 IMU");
  auto yaw_angle = builder.GetEntry("Yaw Angle").GetHandle();
-  builder.SetUpdateTable(
-      [=]() { nt::NetworkTableEntry(yaw_angle).SetDouble(GetAngle()); });
+  builder.SetUpdateTable([=]() {
+    nt::NetworkTableEntry(yaw_angle).SetDouble(GetAngle().value());
+  });
 }
--- a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp
+++ b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp
@@ -79,12 +79,6 @@ ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, SPI::Port port,
        m_simDevice.CreateDouble("gyro_angle_y", hal::SimDevice::kInput, 0.0);
    m_simGyroAngleZ =
        m_simDevice.CreateDouble("gyro_angle_z", hal::SimDevice::kInput, 0.0);
-    m_simGyroRateX =
-        m_simDevice.CreateDouble("gyro_rate_x", hal::SimDevice::kInput, 0.0);
-    m_simGyroRateY =
-        m_simDevice.CreateDouble("gyro_rate_y", hal::SimDevice::kInput, 0.0);
-    m_simGyroRateZ =
-        m_simDevice.CreateDouble("gyro_rate_z", hal::SimDevice::kInput, 0.0);
    m_simAccelX =
        m_simDevice.CreateDouble("accel_x", hal::SimDevice::kInput, 0.0);
    m_simAccelY =
@@ -385,22 +379,6 @@ void ADIS16470_IMU::Calibrate() {
  }
 }

-int ADIS16470_IMU::SetYawAxis(IMUAxis yaw_axis) {
-  if (m_yaw_axis == yaw_axis) {
-    return 1;
-  }
-  if (!SwitchToStandardSPI()) {
-    REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
-    return 2;
-  }
-  m_yaw_axis = yaw_axis;
-  if (!SwitchToAutoSPI()) {
-    REPORT_ERROR("Failed to configure/reconfigure auto SPI.");
-    return 2;
-  }
-  return 0;
-}
-
 /**
 * @brief Reads the contents of a specified register location over SPI.
 *
@@ -733,112 +711,90 @@ double ADIS16470_IMU::CompFilterProcess(double compAngle, double accelAngle,
 *m_yaw_axis. This function is most useful in situations where the yaw axis may
 *not coincide with the IMU Z axis.
 **/
-double ADIS16470_IMU::GetAngle() const {
+units::degree_t ADIS16470_IMU::GetAngle() const {
  switch (m_yaw_axis) {
    case kX:
      if (m_simGyroAngleX) {
-        return m_simGyroAngleX.Get();
+        return units::degree_t{m_simGyroAngleX.Get()};
      }
      break;
    case kY:
      if (m_simGyroAngleY) {
-        return m_simGyroAngleY.Get();
+        return units::degree_t{m_simGyroAngleY.Get()};
      }
      break;
    case kZ:
      if (m_simGyroAngleZ) {
-        return m_simGyroAngleZ.Get();
+        return units::degree_t{m_simGyroAngleZ.Get()};
      }
      break;
  }
  std::scoped_lock sync(m_mutex);
-  return m_integ_angle;
+  return units::degree_t{m_integ_angle};
 }

-double ADIS16470_IMU::GetRate() const {
-  std::scoped_lock sync(m_mutex);
-  switch (m_yaw_axis) {
-    case kX:
-      return GetGyroRateX();
-    case kY:
-      return GetGyroRateY();
-    case kZ:
-      return GetGyroRateZ();
-    default:
-      return 0.0;
+units::meters_per_second_squared_t ADIS16470_IMU::GetAccelX() const {
+  if (m_simAccelX) {
+    return units::meters_per_second_squared_t{m_simAccelX.Get()};
  }
+  std::scoped_lock sync(m_mutex);
+  return units::meters_per_second_squared_t{m_accel_x};
+}
+
+units::meters_per_second_squared_t ADIS16470_IMU::GetAccelY() const {
+  if (m_simAccelY) {
+    return units::meters_per_second_squared_t{m_simAccelY.Get()};
+  }
+  std::scoped_lock sync(m_mutex);
+  return units::meters_per_second_squared_t{m_accel_y};
+}
+
+units::meters_per_second_squared_t ADIS16470_IMU::GetAccelZ() const {
+  if (m_simAccelZ) {
+    return units::meters_per_second_squared_t{m_simAccelZ.Get()};
+  }
+  std::scoped_lock sync(m_mutex);
+  return units::meters_per_second_squared_t{m_accel_z};
+}
+
+units::degree_t ADIS16470_IMU::GetXComplementaryAngle() const {
+  std::scoped_lock sync(m_mutex);
+  return units::degree_t{m_compAngleX};
+}
+
+units::degree_t ADIS16470_IMU::GetYComplementaryAngle() const {
+  std::scoped_lock sync(m_mutex);
+  return units::degree_t{m_compAngleY};
+}
+
+units::degree_t ADIS16470_IMU::GetXFilteredAccelAngle() const {
+  std::scoped_lock sync(m_mutex);
+  return units::degree_t{m_accelAngleX};
+}
+
+units::degree_t ADIS16470_IMU::GetYFilteredAccelAngle() const {
+  std::scoped_lock sync(m_mutex);
+  return units::degree_t{m_accelAngleY};
 }

 ADIS16470_IMU::IMUAxis ADIS16470_IMU::GetYawAxis() const {
  return m_yaw_axis;
 }

-double ADIS16470_IMU::GetGyroRateX() const {
-  if (m_simGyroRateX) {
-    return m_simGyroRateX.Get();
+int ADIS16470_IMU::SetYawAxis(IMUAxis yaw_axis) {
+  if (m_yaw_axis == yaw_axis) {
+    return 1;
  }
-  std::scoped_lock sync(m_mutex);
-  return m_gyro_x;
-}
-
-double ADIS16470_IMU::GetGyroRateY() const {
-  if (m_simGyroRateY) {
-    return m_simGyroRateY.Get();
+  if (!SwitchToStandardSPI()) {
+    REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
+    return 2;
  }
-  std::scoped_lock sync(m_mutex);
-  return m_gyro_y;
-}
-
-double ADIS16470_IMU::GetGyroRateZ() const {
-  if (m_simGyroRateZ) {
-    return m_simGyroRateZ.Get();
+  m_yaw_axis = yaw_axis;
+  if (!SwitchToAutoSPI()) {
+    REPORT_ERROR("Failed to configure/reconfigure auto SPI.");
+    return 2;
  }
-  std::scoped_lock sync(m_mutex);
-  return m_gyro_z;
-}
-
-double ADIS16470_IMU::GetAccelX() const {
-  if (m_simAccelX) {
-    return m_simAccelX.Get();
-  }
-  std::scoped_lock sync(m_mutex);
-  return m_accel_x;
-}
-
-double ADIS16470_IMU::GetAccelY() const {
-  if (m_simAccelY) {
-    return m_simAccelY.Get();
-  }
-  std::scoped_lock sync(m_mutex);
-  return m_accel_y;
-}
-
-double ADIS16470_IMU::GetAccelZ() const {
-  if (m_simAccelZ) {
-    return m_simAccelZ.Get();
-  }
-  std::scoped_lock sync(m_mutex);
-  return m_accel_z;
-}
-
-double ADIS16470_IMU::GetXComplementaryAngle() const {
-  std::scoped_lock sync(m_mutex);
-  return m_compAngleX;
-}
-
-double ADIS16470_IMU::GetYComplementaryAngle() const {
-  std::scoped_lock sync(m_mutex);
-  return m_compAngleY;
-}
-
-double ADIS16470_IMU::GetXFilteredAccelAngle() const {
-  std::scoped_lock sync(m_mutex);
-  return m_accelAngleX;
-}
-
-double ADIS16470_IMU::GetYFilteredAccelAngle() const {
-  std::scoped_lock sync(m_mutex);
-  return m_accelAngleY;
+  return 0;
 }

 int ADIS16470_IMU::GetPort() const {
@@ -854,6 +810,7 @@ int ADIS16470_IMU::GetPort() const {
 void ADIS16470_IMU::InitSendable(nt::NTSendableBuilder& builder) {
  builder.SetSmartDashboardType("ADIS16470 IMU");
  auto yaw_angle = builder.GetEntry("Yaw Angle").GetHandle();
-  builder.SetUpdateTable(
-      [=]() { nt::NetworkTableEntry(yaw_angle).SetDouble(GetAngle()); });
+  builder.SetUpdateTable([=]() {
+    nt::NetworkTableEntry(yaw_angle).SetDouble(GetAngle().value());
+  });
 }
--- a/wpilibc/src/main/native/cpp/simulation/ADIS16448_IMUSim.cpp
+++ b/wpilibc/src/main/native/cpp/simulation/ADIS16448_IMUSim.cpp
@@ -14,9 +14,6 @@ ADIS16448_IMUSim::ADIS16448_IMUSim(const frc::ADIS16448_IMU& imu) {
  m_simGyroAngleX = deviceSim.GetDouble("gyro_angle_x");
  m_simGyroAngleY = deviceSim.GetDouble("gyro_angle_y");
  m_simGyroAngleZ = deviceSim.GetDouble("gyro_angle_z");
-  m_simGyroRateX = deviceSim.GetDouble("gyro_rate_x");
-  m_simGyroRateY = deviceSim.GetDouble("gyro_rate_y");
-  m_simGyroRateZ = deviceSim.GetDouble("gyro_rate_z");
  m_simAccelX = deviceSim.GetDouble("accel_x");
  m_simAccelY = deviceSim.GetDouble("accel_y");
  m_simAccelZ = deviceSim.GetDouble("accel_z");
@@ -34,26 +31,14 @@ void ADIS16448_IMUSim::SetGyroAngleZ(units::degree_t angle) {
  m_simGyroAngleZ.Set(angle.value());
 }

-void ADIS16448_IMUSim::SetGyroRateX(units::degrees_per_second_t rate) {
-  m_simGyroRateX.Set(rate.value());
-}
-
-void ADIS16448_IMUSim::SetGyroRateY(units::degrees_per_second_t rate) {
-  m_simGyroRateY.Set(rate.value());
-}
-
-void ADIS16448_IMUSim::SetGyroRateZ(units::degrees_per_second_t rate) {
-  m_simGyroRateZ.Set(rate.value());
-}
-
 void ADIS16448_IMUSim::SetAccelX(units::meters_per_second_squared_t accel) {
-  m_simAccelX.Set(accel.value() / 9.81);
+  m_simAccelX.Set(accel.value());
 }

 void ADIS16448_IMUSim::SetAccelY(units::meters_per_second_squared_t accel) {
-  m_simAccelY.Set(accel.value() / 9.81);
+  m_simAccelY.Set(accel.value());
 }

 void ADIS16448_IMUSim::SetAccelZ(units::meters_per_second_squared_t accel) {
-  m_simAccelZ.Set(accel.value() / 9.81);
+  m_simAccelZ.Set(accel.value());
 }
--- a/wpilibc/src/main/native/cpp/simulation/ADIS16470_IMUSim.cpp
+++ b/wpilibc/src/main/native/cpp/simulation/ADIS16470_IMUSim.cpp
@@ -14,9 +14,6 @@ ADIS16470_IMUSim::ADIS16470_IMUSim(const frc::ADIS16470_IMU& imu) {
  m_simGyroAngleX = deviceSim.GetDouble("gyro_angle_x");
  m_simGyroAngleY = deviceSim.GetDouble("gyro_angle_y");
  m_simGyroAngleZ = deviceSim.GetDouble("gyro_angle_z");
-  m_simGyroRateX = deviceSim.GetDouble("gyro_rate_x");
-  m_simGyroRateY = deviceSim.GetDouble("gyro_rate_y");
-  m_simGyroRateZ = deviceSim.GetDouble("gyro_rate_z");
  m_simAccelX = deviceSim.GetDouble("accel_x");
  m_simAccelY = deviceSim.GetDouble("accel_y");
  m_simAccelZ = deviceSim.GetDouble("accel_z");
@@ -34,26 +31,14 @@ void ADIS16470_IMUSim::SetGyroAngleZ(units::degree_t angle) {
  m_simGyroAngleZ.Set(angle.value());
 }

-void ADIS16470_IMUSim::SetGyroRateX(units::degrees_per_second_t rate) {
-  m_simGyroRateX.Set(rate.value());
-}
-
-void ADIS16470_IMUSim::SetGyroRateY(units::degrees_per_second_t rate) {
-  m_simGyroRateY.Set(rate.value());
-}
-
-void ADIS16470_IMUSim::SetGyroRateZ(units::degrees_per_second_t rate) {
-  m_simGyroRateZ.Set(rate.value());
-}
-
 void ADIS16470_IMUSim::SetAccelX(units::meters_per_second_squared_t accel) {
-  m_simAccelX.Set(accel.value() / 9.81);
+  m_simAccelX.Set(accel.value());
 }

 void ADIS16470_IMUSim::SetAccelY(units::meters_per_second_squared_t accel) {
-  m_simAccelY.Set(accel.value() / 9.81);
+  m_simAccelY.Set(accel.value());
 }

 void ADIS16470_IMUSim::SetAccelZ(units::meters_per_second_squared_t accel) {
-  m_simAccelZ.Set(accel.value() / 9.81);
+  m_simAccelZ.Set(accel.value());
 }
--- a/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h
+++ b/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h
@@ -13,10 +13,6 @@

 #pragma once

-#include <frc/DigitalInput.h>
-#include <frc/DigitalOutput.h>
-#include <frc/DigitalSource.h>
-#include <frc/SPI.h>
 #include <stdint.h>

 #include <atomic>
@@ -25,10 +21,21 @@

 #include <hal/SimDevice.h>
 #include <networktables/NTSendable.h>
+#include <units/acceleration.h>
+#include <units/angle.h>
+#include <units/angular_velocity.h>
+#include <units/magnetic_field_strength.h>
+#include <units/pressure.h>
+#include <units/temperature.h>
 #include <wpi/condition_variable.h>
 #include <wpi/mutex.h>
 #include <wpi/sendable/SendableHelper.h>

+#include "frc/DigitalInput.h"
+#include "frc/DigitalOutput.h"
+#include "frc/DigitalSource.h"
+#include "frc/SPI.h"
+
 namespace frc {
 /**
 * Use DMA SPI to read rate, acceleration, and magnetometer data from the
@@ -49,6 +56,22 @@ namespace frc {
 class ADIS16448_IMU : public nt::NTSendable,
                      public wpi::SendableHelper<ADIS16448_IMU> {
 public:
+  /* ADIS16448 Calibration Time Enum Class */
+  enum class CalibrationTime {
+    _32ms = 0,
+    _64ms = 1,
+    _128ms = 2,
+    _256ms = 3,
+    _512ms = 4,
+    _1s = 5,
+    _2s = 6,
+    _4s = 7,
+    _8s = 8,
+    _16s = 9,
+    _32s = 10,
+    _64s = 11
+  };
+
  enum IMUAxis { kX, kY, kZ };

  /**
@@ -65,7 +88,8 @@ class ADIS16448_IMU : public nt::NTSendable,
   * @param port The SPI port where the IMU is connected.
   * @param cal_time The calibration time that should be used on start-up.
   */
-  explicit ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port, uint16_t cal_time);
+  explicit ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port,
+                         CalibrationTime cal_time);

  ~ADIS16448_IMU() override;

@@ -93,7 +117,7 @@ class ADIS16448_IMU : public nt::NTSendable,
   *
   * @param new_cal_time The calibration time that should be used
   */
-  int ConfigCalTime(int new_cal_time);
+  int ConfigCalTime(CalibrationTime new_cal_time);

  /**
   * Reset the gyro.
@@ -105,64 +129,57 @@ class ADIS16448_IMU : public nt::NTSendable,
  void Reset();

  /**
-   * Return the actual angle in degrees that the robot is currently facing.
-   *
-   * The angle is based on the current accumulator value corrected by
-   * offset calibration and built-in IMU calibration. The angle is continuous,
-   * that is it will continue from 360->361 degrees. This allows algorithms
-   * that wouldn't want to see a discontinuity in the gyro output as it sweeps
-   * from 360 to 0 on the second time around. The axis returned by this
-   * function is adjusted fased on the configured yaw_axis.
-   *
-   * @return the current heading of the robot in degrees. This heading is based
-   *         on integration of the returned rate from the gyro.
+   * Returns the yaw axis angle in degrees (CCW positive).
   */
-  double GetAngle() const;
+  units::degree_t GetAngle() const;

  /**
-   * Return the rate of rotation of the yaw_axis gyro.
-   *
-   * The rate is based on the most recent reading of the gyro value
-   *
-   * @return the current rate in degrees per second
+   * Returns the accumulated gyro angle in the X axis.
   */
-  double GetRate() const;
+  units::degree_t GetGyroAngleX() const;

-  double GetGyroAngleX() const;
+  /**
+   * Returns the accumulated gyro angle in the Y axis.
+   */
+  units::degree_t GetGyroAngleY() const;

-  double GetGyroAngleY() const;
+  /**
+   * Returns the accumulated gyro angle in the Z axis.
+   */
+  units::degree_t GetGyroAngleZ() const;

-  double GetGyroAngleZ() const;
+  /**
+   * Returns the acceleration in the X axis.
+   */
+  units::meters_per_second_squared_t GetAccelX() const;

-  double GetGyroRateX() const;
+  /**
+   * Returns the acceleration in the Y axis.
+   */
+  units::meters_per_second_squared_t GetAccelY() const;

-  double GetGyroRateY() const;
+  /**
+   * Returns the acceleration in the Z axis.
+   */
+  units::meters_per_second_squared_t GetAccelZ() const;

-  double GetGyroRateZ() const;
+  units::degree_t GetXComplementaryAngle() const;

-  double GetAccelX() const;
+  units::degree_t GetYComplementaryAngle() const;

-  double GetAccelY() const;
+  units::degree_t GetXFilteredAccelAngle() const;

-  double GetAccelZ() const;
+  units::degree_t GetYFilteredAccelAngle() const;

-  double GetXComplementaryAngle() const;
+  units::tesla_t GetMagneticFieldX() const;

-  double GetYComplementaryAngle() const;
+  units::tesla_t GetMagneticFieldY() const;

-  double GetXFilteredAccelAngle() const;
+  units::tesla_t GetMagneticFieldZ() const;

-  double GetYFilteredAccelAngle() const;
+  units::pounds_per_square_inch_t GetBarometricPressure() const;

-  double GetMagInstantX() const;
-
-  double GetMagInstantY() const;
-
-  double GetMagInstantZ() const;
-
-  double GetBarometricPressure() const;
-
-  double GetTemperature() const;
+  units::celsius_t GetTemperature() const;

  IMUAxis GetYawAxis() const;

@@ -317,7 +334,7 @@ class ADIS16448_IMU : public nt::NTSendable,

  bool m_auto_configured = false;
  SPI::Port m_spi_port;
-  uint16_t m_calibration_time;
+  CalibrationTime m_calibration_time;
  SPI* m_spi = nullptr;
  DigitalInput* m_auto_interrupt = nullptr;

@@ -327,9 +344,6 @@ class ADIS16448_IMU : public nt::NTSendable,
  hal::SimDouble m_simGyroAngleX;
  hal::SimDouble m_simGyroAngleY;
  hal::SimDouble m_simGyroAngleZ;
-  hal::SimDouble m_simGyroRateX;
-  hal::SimDouble m_simGyroRateY;
-  hal::SimDouble m_simGyroRateZ;
  hal::SimDouble m_simAccelX;
  hal::SimDouble m_simAccelY;
  hal::SimDouble m_simAccelZ;
--- a/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h
+++ b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h
@@ -13,10 +13,6 @@

 #pragma once

-#include <frc/DigitalInput.h>
-#include <frc/DigitalOutput.h>
-#include <frc/DigitalSource.h>
-#include <frc/SPI.h>
 #include <stdint.h>

 #include <atomic>
@@ -25,10 +21,18 @@

 #include <hal/SimDevice.h>
 #include <networktables/NTSendable.h>
+#include <units/acceleration.h>
+#include <units/angle.h>
+#include <units/angular_velocity.h>
 #include <wpi/condition_variable.h>
 #include <wpi/mutex.h>
 #include <wpi/sendable/SendableHelper.h>

+#include "frc/DigitalInput.h"
+#include "frc/DigitalOutput.h"
+#include "frc/DigitalSource.h"
+#include "frc/SPI.h"
+
 namespace frc {
 /**
 * Use DMA SPI to read rate and acceleration data from the ADIS16470 IMU and
@@ -119,41 +123,32 @@ class ADIS16470_IMU : public nt::NTSendable,
  void Reset();

  /**
-   * @brief Returns the current integrated angle for the axis specified.
-   *
-   * The angle is based on the current accumulator value corrected by
-   * offset calibration and built-in IMU calibration. The angle is continuous,
-   * that is it will continue from 360->361 degrees. This allows algorithms
-   * that wouldn't want to see a discontinuity in the gyro output as it sweeps
-   * from 360 to 0 on the second time around. The axis returned by this
-   * function is adjusted based on the configured yaw_axis.
-   *
-   * @return the current heading of the robot in degrees. This heading is based
-   *         on integration of the returned rate from the gyro.
+   * Returns the yaw axis angle in degrees (CCW positive).
   */
-  double GetAngle() const;
+  units::degree_t GetAngle() const;

-  double GetRate() const;
+  /**
+   * Returns the acceleration in the X axis.
+   */
+  units::meters_per_second_squared_t GetAccelX() const;

-  double GetGyroRateX() const;
+  /**
+   * Returns the acceleration in the Y axis.
+   */
+  units::meters_per_second_squared_t GetAccelY() const;

-  double GetGyroRateY() const;
+  /**
+   * Returns the acceleration in the Z axis.
+   */
+  units::meters_per_second_squared_t GetAccelZ() const;

-  double GetGyroRateZ() const;
+  units::degree_t GetXComplementaryAngle() const;

-  double GetAccelX() const;
+  units::degree_t GetYComplementaryAngle() const;

-  double GetAccelY() const;
+  units::degree_t GetXFilteredAccelAngle() const;

-  double GetAccelZ() const;
-
-  double GetXComplementaryAngle() const;
-
-  double GetYComplementaryAngle() const;
-
-  double GetXFilteredAccelAngle() const;
-
-  double GetYFilteredAccelAngle() const;
+  units::degree_t GetYFilteredAccelAngle() const;

  IMUAxis GetYawAxis() const;

@@ -380,9 +375,6 @@ class ADIS16470_IMU : public nt::NTSendable,
  hal::SimDouble m_simGyroAngleX;
  hal::SimDouble m_simGyroAngleY;
  hal::SimDouble m_simGyroAngleZ;
-  hal::SimDouble m_simGyroRateX;
-  hal::SimDouble m_simGyroRateY;
-  hal::SimDouble m_simGyroRateZ;
  hal::SimDouble m_simAccelX;
  hal::SimDouble m_simAccelY;
  hal::SimDouble m_simAccelZ;
--- a/wpilibc/src/main/native/include/frc/simulation/ADIS16448_IMUSim.h
+++ b/wpilibc/src/main/native/include/frc/simulation/ADIS16448_IMUSim.h
@@ -48,27 +48,6 @@ class ADIS16448_IMUSim {
   */
  void SetGyroAngleZ(units::degree_t angle);

-  /**
-   * Sets the X axis angular rate (CCW positive).
-   *
-   * @param rate The angular rate.
-   */
-  void SetGyroRateX(units::degrees_per_second_t rate);
-
-  /**
-   * Sets the Y axis angular rate (CCW positive).
-   *
-   * @param rate The angular rate.
-   */
-  void SetGyroRateY(units::degrees_per_second_t rate);
-
-  /**
-   * Sets the Z axis angular rate (CCW positive).
-   *
-   * @param rate The angular rate.
-   */
-  void SetGyroRateZ(units::degrees_per_second_t rate);
-
  /**
   * Sets the X axis acceleration.
   *
@@ -94,9 +73,6 @@ class ADIS16448_IMUSim {
  hal::SimDouble m_simGyroAngleX;
  hal::SimDouble m_simGyroAngleY;
  hal::SimDouble m_simGyroAngleZ;
-  hal::SimDouble m_simGyroRateX;
-  hal::SimDouble m_simGyroRateY;
-  hal::SimDouble m_simGyroRateZ;
  hal::SimDouble m_simAccelX;
  hal::SimDouble m_simAccelY;
  hal::SimDouble m_simAccelZ;
--- a/wpilibc/src/main/native/include/frc/simulation/ADIS16470_IMUSim.h
+++ b/wpilibc/src/main/native/include/frc/simulation/ADIS16470_IMUSim.h
@@ -48,27 +48,6 @@ class ADIS16470_IMUSim {
   */
  void SetGyroAngleZ(units::degree_t angle);

-  /**
-   * Sets the X axis angular rate (CCW positive).
-   *
-   * @param rate The angular rate.
-   */
-  void SetGyroRateX(units::degrees_per_second_t rate);
-
-  /**
-   * Sets the Y axis angular rate (CCW positive).
-   *
-   * @param rate The angular rate.
-   */
-  void SetGyroRateY(units::degrees_per_second_t rate);
-
-  /**
-   * Sets the Z axis angular rate (CCW positive).
-   *
-   * @param rate The angular rate.
-   */
-  void SetGyroRateZ(units::degrees_per_second_t rate);
-
  /**
   * Sets the X axis acceleration.
   *
@@ -94,9 +73,6 @@ class ADIS16470_IMUSim {
  hal::SimDouble m_simGyroAngleX;
  hal::SimDouble m_simGyroAngleY;
  hal::SimDouble m_simGyroAngleZ;
-  hal::SimDouble m_simGyroRateX;
-  hal::SimDouble m_simGyroRateY;
-  hal::SimDouble m_simGyroRateZ;
  hal::SimDouble m_simAccelX;
  hal::SimDouble m_simAccelY;
  hal::SimDouble m_simAccelZ;