From 3d32efc77618adbd7bc9a33a3b9a7b230a1fe834 Mon Sep 17 00:00:00 2001
From: Gold856 <117957790+Gold856@users.noreply.github.com>
Date: Wed, 5 Jun 2024 01:51:28 -0400
Subject: [PATCH] Refactor and clean up ADIS IMU classes

---
 wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp | 274 ++++------
 wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp | 240 +++------
 .../main/native/include/frc/ADIS16448_IMU.h   |   7 +-
 .../main/native/include/frc/ADIS16470_IMU.h   |  11 +-
 .../edu/wpi/first/wpilibj/ADIS16448_IMU.java  | 505 ++++++------------
 .../edu/wpi/first/wpilibj/ADIS16470_IMU.java  | 476 ++++-------------
 6 files changed, 437 insertions(+), 1076 deletions(-)

diff --git a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp
index 8ba4a96c40..88bb5dc90a 100644
--- a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp
+++ b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp
@@ -29,16 +29,13 @@
 #include <wpi/sendable/SendableRegistry.h>
 
 #include "frc/Errors.h"
+#include "frc/MathUtil.h"
 
 /* Helpful conversion functions */
 static inline uint16_t BuffToUShort(const uint32_t* buf) {
   return (static_cast<uint16_t>(buf[0]) << 8) | buf[1];
 }
 
-static inline uint8_t BuffToUByte(const uint32_t* buf) {
-  return static_cast<uint8_t>(buf[0]);
-}
-
 static inline int16_t BuffToShort(const uint32_t* buf) {
   return (static_cast<int16_t>(buf[0]) << 8) | buf[1];
 }
@@ -100,11 +97,11 @@ ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port,
     // Relies on the RIO hardware by default configuring an output as low
     // and configuring an input as high Z. The 10k pull-up resistor internal to
     // the IMU then forces the reset line high for normal operation.
-    DigitalOutput* m_reset_out = new DigitalOutput(18);  // Drive MXP DIO8 low
-    Wait(10_ms);                                         // Wait 10ms
-    delete m_reset_out;
+    DigitalOutput* reset_out = new DigitalOutput(18);  // Drive MXP DIO8 low
+    Wait(10_ms);
+    delete reset_out;
     m_reset_in = new DigitalInput(18);  // Set MXP DIO8 high
-    Wait(500_ms);                       // Wait 500ms for reset to complete
+    Wait(500_ms);                       // Wait for reset to complete
 
     ConfigCalTime(cal_time);
 
@@ -112,15 +109,12 @@ ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port,
     if (!SwitchToStandardSPI()) {
       return;
     }
-
-    // Set up flash state variable
-    bool m_needs_flash = false;
-
+    bool needsFlash = false;
     // Set IMU internal decimation to 1 (output data rate of 819.2 SPS / (1 + 1)
     // = 409.6Hz), output bandwidth = 204.8Hz
     if (ReadRegister(SMPL_PRD) != 0x0001) {
       WriteRegister(SMPL_PRD, 0x0001);
-      m_needs_flash = true;
+      needsFlash = true;
       REPORT_WARNING(
           "ADIS16448: SMPL_PRD register configuration inconsistent! Scheduling "
           "flash update.");
@@ -129,7 +123,7 @@ ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port,
     // Set data ready polarity (LOW = Good Data) on DIO1 (PWM0 on MXP)
     if (ReadRegister(MSC_CTRL) != 0x0016) {
       WriteRegister(MSC_CTRL, 0x0016);
-      m_needs_flash = true;
+      needsFlash = true;
       REPORT_WARNING(
           "ADIS16448: MSC_CTRL register configuration inconsistent! Scheduling "
           "flash update.");
@@ -139,7 +133,7 @@ ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port,
     // set IMU scale factor (range)
     if (ReadRegister(SENS_AVG) != 0x0400) {
       WriteRegister(SENS_AVG, 0x0400);
-      m_needs_flash = true;
+      needsFlash = true;
       REPORT_WARNING(
           "ADIS16448: SENS_AVG register configuration inconsistent! Scheduling "
           "flash update.");
@@ -147,7 +141,7 @@ ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port,
     // Clear offset registers
     if (ReadRegister(XGYRO_OFF) != 0x0000) {
       WriteRegister(XGYRO_OFF, 0x0000);
-      m_needs_flash = true;
+      needsFlash = true;
       REPORT_WARNING(
           "ADIS16448: XGYRO_OFF register configuration inconsistent! "
           "Scheduling flash update.");
@@ -155,7 +149,7 @@ ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port,
 
     if (ReadRegister(YGYRO_OFF) != 0x0000) {
       WriteRegister(YGYRO_OFF, 0x0000);
-      m_needs_flash = true;
+      needsFlash = true;
       REPORT_WARNING(
           "ADIS16448: YGYRO_OFF register configuration inconsistent! "
           "Scheduling flash update.");
@@ -163,7 +157,7 @@ ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port,
 
     if (ReadRegister(ZGYRO_OFF) != 0x0000) {
       WriteRegister(ZGYRO_OFF, 0x0000);
-      m_needs_flash = true;
+      needsFlash = true;
       REPORT_WARNING(
           "ADIS16448: ZGYRO_OFF register configuration inconsistent! "
           "Scheduling flash update.");
@@ -171,16 +165,15 @@ ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port,
 
     // If any registers on the IMU don't match the config, trigger a flash
     // update
-    if (m_needs_flash) {
+    if (needsFlash) {
       REPORT_WARNING(
           "ADIS16448: Register configuration changed! Starting IMU flash "
           "update.");
       WriteRegister(GLOB_CMD, 0x0008);
-      // Wait long enough for the flash update to finish (72ms minimum as per
+      // Wait long enough for the flash update to finish (75ms minimum as per
       // the datasheet)
       Wait(0.5_s);
       REPORT_WARNING("ADIS16448: Flash update finished!");
-      m_needs_flash = false;
     } else {
       REPORT_WARNING(
           "ADIS16448: Flash and RAM configuration consistent. No flash update "
@@ -202,11 +195,11 @@ ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port,
     // Tell the acquire loop that we're done starting up
     m_start_up_mode = false;
 
-    // Let the user know the IMU was initiallized successfully
+    // Let the user know the IMU was initialized successfully
     REPORT_WARNING("ADIS16448 IMU Successfully Initialized!");
 
     // TODO: Find what the proper pin is to turn this LED
-    //  Drive MXP PWM5 (IMU ready LED) low (active low)
+    // Drive MXP PWM5 (IMU ready LED) low (active low)
     m_status_led = new DigitalOutput(19);
   }
 
@@ -232,7 +225,6 @@ ADIS16448_IMU::ADIS16448_IMU(ADIS16448_IMU&& other)
       m_mag_z{std::move(other.m_mag_z)},
       m_baro{std::move(other.m_baro)},
       m_temp{std::move(other.m_temp)},
-      m_tau{std::move(other.m_tau)},
       m_dt{std::move(other.m_dt)},
       m_alpha{std::move(other.m_alpha)},
       m_compAngleX{std::move(other.m_compAngleX)},
@@ -291,7 +283,6 @@ ADIS16448_IMU& ADIS16448_IMU::operator=(ADIS16448_IMU&& other) {
   std::swap(this->m_mag_z, other.m_mag_z);
   std::swap(this->m_baro, other.m_baro);
   std::swap(this->m_temp, other.m_temp);
-  std::swap(this->m_tau, other.m_tau);
   std::swap(this->m_dt, other.m_dt);
   std::swap(this->m_alpha, other.m_alpha);
   std::swap(this->m_compAngleX, other.m_compAngleX);
@@ -365,18 +356,18 @@ bool ADIS16448_IMU::SwitchToStandardSPI() {
     if (m_spi != nullptr && m_auto_configured) {
       m_spi->StopAuto();
       // We need to get rid of all the garbage left in the auto SPI buffer after
-      // stopping it. Sometimes data magically reappears, so we have to check
-      // the buffer size a couple of times
-      //  to be sure we got it all. Yuck.
+      // stopping it.
+      // Sometimes data magically reappears, so we have to check
+      // the buffer size a couple of times to be sure we got it all. Yuck.
       uint32_t trashBuffer[200];
       Wait(100_ms);
       int data_count = m_spi->ReadAutoReceivedData(trashBuffer, 0, 0_s);
       while (data_count > 0) {
-        /* Dequeue 200 at a time, or the remainder of the buffer if less than
-         * 200 */
+        // Dequeue 200 at a time, or the remainder of the buffer if less than
+        // 200
         m_spi->ReadAutoReceivedData(trashBuffer, (std::min)(200, data_count),
                                     0_s);
-        /* Update remaining buffer count */
+        // Update remaining buffer count
         data_count = m_spi->ReadAutoReceivedData(trashBuffer, 0, 0_s);
       }
     }
@@ -443,11 +434,9 @@ void ADIS16448_IMU::InitOffsetBuffer(int size) {
  **/
 bool ADIS16448_IMU::SwitchToAutoSPI() {
   // No SPI port has been set up. Go set one up first.
-  if (m_spi == nullptr) {
-    if (!SwitchToStandardSPI()) {
-      REPORT_ERROR("Failed to start/restart auto SPI");
-      return false;
-    }
+  if (m_spi == nullptr && !SwitchToStandardSPI()) {
+    REPORT_ERROR("Failed to start/restart auto SPI");
+    return false;
   }
   // Only set up the interrupt if needed.
   if (m_auto_interrupt == nullptr) {
@@ -489,9 +478,6 @@ bool ADIS16448_IMU::SwitchToAutoSPI() {
   return true;
 }
 
-/**
- *
- **/
 int ADIS16448_IMU::ConfigCalTime(CalibrationTime new_cal_time) {
   if (m_calibration_time == new_cal_time) {
     return 1;
@@ -503,9 +489,6 @@ int ADIS16448_IMU::ConfigCalTime(CalibrationTime new_cal_time) {
   }
 }
 
-/**
- *
- **/
 void ADIS16448_IMU::Calibrate() {
   std::scoped_lock sync(m_mutex);
   // Calculate the running average
@@ -547,9 +530,9 @@ uint16_t ADIS16448_IMU::ReadRegister(uint8_t reg) {
 /**
  * This function writes an unsigned, 16-bit value into adjacent 8-bit addresses
  *via SPI. The upper and lower bytes that make up the 16-bit value are split
- *into two unsined, 8-bit values and written to the upper and lower addresses of
- *the specified register value. Only the lower (base) address must be specified.
- *This function assumes the controller is set to standard SPI mode.
+ *into two unsigned, 8-bit values and written to the upper and lower addresses
+ *of the specified register value. Only the lower (base) address must be
+ *specified. This function assumes the controller is set to standard SPI mode.
  **/
 void ADIS16448_IMU::WriteRegister(uint8_t reg, uint16_t val) {
   uint8_t buf[2];
@@ -609,126 +592,95 @@ ADIS16448_IMU::~ADIS16448_IMU() {
 void ADIS16448_IMU::Acquire() {
   // Set data packet length
   const int dataset_len = 29;  // 18 data points + timestamp
-
   const int BUFFER_SIZE = 4000;
 
   // This buffer can contain many datasets
   uint32_t buffer[BUFFER_SIZE];
-  int data_count = 0;
-  int data_remainder = 0;
-  int data_to_read = 0;
-  int bufferAvgIndex = 0;
   uint32_t previous_timestamp = 0;
-  double gyro_rate_x = 0.0;
-  double gyro_rate_y = 0.0;
-  double gyro_rate_z = 0.0;
-  double accel_x = 0.0;
-  double accel_y = 0.0;
-  double accel_z = 0.0;
-  double mag_x = 0.0;
-  double mag_y = 0.0;
-  double mag_z = 0.0;
-  double baro = 0.0;
-  double temp = 0.0;
-  double gyro_rate_x_si = 0.0;
-  double gyro_rate_y_si = 0.0;
-  // double gyro_rate_z_si = 0.0;
-  double accel_x_si = 0.0;
-  double accel_y_si = 0.0;
-  double accel_z_si = 0.0;
   double compAngleX = 0.0;
   double compAngleY = 0.0;
-  double accelAngleX = 0.0;
-  double accelAngleY = 0.0;
-
   while (true) {
-    // Sleep loop for 10ms (wait for data)
+    // Wait for data
     Wait(10_ms);
 
     if (m_thread_active) {
-      data_count = m_spi->ReadAutoReceivedData(
-          buffer, 0,
-          0_s);  // Read number of bytes currently stored in the buffer
-      data_remainder =
-          data_count % dataset_len;  // Check if frame is incomplete
-      data_to_read = data_count -
-                     data_remainder;  // Remove incomplete data from read count
-      /* Want to cap the data to read in a single read at the buffer size */
+      // Read number of bytes currently stored in the buffer
+      int data_count = m_spi->ReadAutoReceivedData(buffer, 0, 0_s);
+      // Check if frame is incomplete
+      int data_remainder = data_count % dataset_len;
+      // Remove incomplete data from read count
+      int data_to_read = data_count - data_remainder;
+      // Want to cap the data to read in a single read at the buffer size
       if (data_to_read > BUFFER_SIZE) {
         REPORT_WARNING(
             "ADIS16448 data processing thread overrun has occurred!");
         data_to_read = BUFFER_SIZE - (BUFFER_SIZE % dataset_len);
       }
-      m_spi->ReadAutoReceivedData(buffer, data_to_read,
-                                  0_s);  // Read data from DMA buffer
+      // Read data from DMA buffer
+      m_spi->ReadAutoReceivedData(buffer, data_to_read, 0_s);
 
       // Could be multiple data sets in the buffer. Handle each one.
       for (int i = 0; i < data_to_read; i += dataset_len) {
         // Calculate CRC-16 on each data packet
         uint16_t calc_crc = 0xFFFF;  // Starting word
-        uint8_t byte = 0;
-        uint16_t imu_crc = 0;
-        for (int k = 5; k < 27;
-             k += 2)  // Cycle through XYZ GYRO, XYZ ACCEL, XYZ MAG, BARO, TEMP
-                      // (Ignore Status & CRC)
-        {
-          byte = BuffToUByte(&buffer[i + k + 1]);  // Process LSB
-          calc_crc = (calc_crc >> 8) ^ adiscrc[(calc_crc & 0x00FF) ^ byte];
-          byte = BuffToUByte(&buffer[i + k]);  // Process MSB
-          calc_crc = (calc_crc >> 8) ^ adiscrc[(calc_crc & 0x00FF) ^ byte];
+        // Cycle through XYZ GYRO, XYZ ACCEL, XYZ MAG, BARO, TEMP (Ignore Status
+        // & CRC)
+        for (int k = 5; k < 27; k += 2) {
+          // Process LSB
+          uint8_t byte = static_cast<uint8_t>(buffer[i + k + 1]);
+          calc_crc = (calc_crc >> 8) ^ m_adiscrc[(calc_crc & 0x00FF) ^ byte];
+          // Process MSB
+          byte = static_cast<uint8_t>(buffer[i + k]);
+          calc_crc = (calc_crc >> 8) ^ m_adiscrc[(calc_crc & 0x00FF) ^ byte];
         }
-        calc_crc = ~calc_crc;  // Complement
-        calc_crc = static_cast<uint16_t>((calc_crc << 8) |
-                                         (calc_crc >> 8));  // Flip LSB & MSB
-        imu_crc =
-            BuffToUShort(&buffer[i + 27]);  // Extract DUT CRC from data buffer
+        // Complement
+        calc_crc = ~calc_crc;
+        // Flip LSB & MSB
+        calc_crc = static_cast<uint16_t>((calc_crc << 8) | (calc_crc >> 8));
+        // Extract DUT CRC from data buffer
+        uint16_t imu_crc = BuffToUShort(&buffer[i + 27]);
 
         // Compare calculated vs read CRC. Don't update outputs or dt if CRC-16
         // is bad
         if (calc_crc == imu_crc) {
           // Timestamp is at buffer[i]
           m_dt = (buffer[i] - previous_timestamp) / 1000000.0;
-          // Split array and scale data
-          gyro_rate_x = BuffToShort(&buffer[i + 5]) * 0.04;
-          gyro_rate_y = BuffToShort(&buffer[i + 7]) * 0.04;
-          gyro_rate_z = BuffToShort(&buffer[i + 9]) * 0.04;
-          accel_x = BuffToShort(&buffer[i + 11]) * 0.833;
-          accel_y = BuffToShort(&buffer[i + 13]) * 0.833;
-          accel_z = BuffToShort(&buffer[i + 15]) * 0.833;
-          mag_x = BuffToShort(&buffer[i + 17]) * 0.1429;
-          mag_y = BuffToShort(&buffer[i + 19]) * 0.1429;
-          mag_z = BuffToShort(&buffer[i + 21]) * 0.1429;
-          baro = BuffToShort(&buffer[i + 23]) * 0.02;
-          temp = BuffToShort(&buffer[i + 25]) * 0.07386 + 31.0;
+          // Scale sensor data
+          double gyro_rate_x = BuffToShort(&buffer[i + 5]) * 0.04;
+          double gyro_rate_y = BuffToShort(&buffer[i + 7]) * 0.04;
+          double gyro_rate_z = BuffToShort(&buffer[i + 9]) * 0.04;
+          double accel_x = BuffToShort(&buffer[i + 11]) * 0.833;
+          double accel_y = BuffToShort(&buffer[i + 13]) * 0.833;
+          double accel_z = BuffToShort(&buffer[i + 15]) * 0.833;
+          double mag_x = BuffToShort(&buffer[i + 17]) * 0.1429;
+          double mag_y = BuffToShort(&buffer[i + 19]) * 0.1429;
+          double mag_z = BuffToShort(&buffer[i + 21]) * 0.1429;
+          double baro = BuffToShort(&buffer[i + 23]) * 0.02;
+          double temp = BuffToShort(&buffer[i + 25]) * 0.07386 + 31.0;
 
           // Convert scaled sensor data to SI units
-          gyro_rate_x_si = gyro_rate_x * deg_to_rad;
-          gyro_rate_y_si = gyro_rate_y * deg_to_rad;
-          // gyro_rate_z_si = gyro_rate_z * deg_to_rad;
-          accel_x_si = accel_x * grav;
-          accel_y_si = accel_y * grav;
-          accel_z_si = accel_z * grav;
+          double gyro_rate_x_si = gyro_rate_x * deg_to_rad;
+          double gyro_rate_y_si = gyro_rate_y * deg_to_rad;
+          // double gyro_rate_z_si = gyro_rate_z * deg_to_rad;
+          double accel_x_si = accel_x * grav;
+          double accel_y_si = accel_y * grav;
+          double accel_z_si = accel_z * grav;
           // Store timestamp for next iteration
           previous_timestamp = buffer[i];
           // Calculate alpha for use with the complementary filter
-          m_alpha = m_tau / (m_tau + m_dt);
+          m_alpha = 0.5 / (0.5 + m_dt);
+          // Run inclinometer calculations
+          double accelAngleX = atan2f(
+              -accel_x_si,
+              sqrtf((accel_y_si * accel_y_si) + (-accel_z_si * -accel_z_si)));
+          double accelAngleY = atan2f(
+              accel_y_si,
+              sqrtf((-accel_x_si * -accel_x_si) + (-accel_z_si * -accel_z_si)));
           // Calculate complementary filter
           if (m_first_run) {
-            accelAngleX = atan2f(
-                -accel_x_si,
-                sqrtf((accel_y_si * accel_y_si) + (-accel_z_si * -accel_z_si)));
-            accelAngleY =
-                atan2f(accel_y_si, sqrtf((-accel_x_si * -accel_x_si) +
-                                         (-accel_z_si * -accel_z_si)));
             compAngleX = accelAngleX;
             compAngleY = accelAngleY;
           } else {
-            accelAngleX = atan2f(
-                -accel_x_si,
-                sqrtf((accel_y_si * accel_y_si) + (-accel_z_si * -accel_z_si)));
-            accelAngleY =
-                atan2f(accel_y_si, sqrtf((-accel_x_si * -accel_x_si) +
-                                         (-accel_z_si * -accel_z_si)));
             accelAngleX = FormatAccelRange(accelAngleX, -accel_z_si);
             accelAngleY = FormatAccelRange(accelAngleY, -accel_z_si);
             compAngleX =
@@ -748,7 +700,7 @@ void ADIS16448_IMU::Acquire() {
             } else {
               // Accumulate gyro for offset calibration
               // Add most recent sample data to buffer
-              bufferAvgIndex = m_accum_count % m_avg_size;
+              int bufferAvgIndex = m_accum_count % m_avg_size;
               m_offset_buffer[bufferAvgIndex] =
                   OffsetData{gyro_rate_x, gyro_rate_y, gyro_rate_z};
               // Increment counter
@@ -787,31 +739,9 @@ void ADIS16448_IMU::Acquire() {
       }
     } else {
       m_thread_idle = true;
-      data_count = 0;
-      data_remainder = 0;
-      data_to_read = 0;
       previous_timestamp = 0.0;
-      gyro_rate_x = 0.0;
-      gyro_rate_y = 0.0;
-      gyro_rate_z = 0.0;
-      accel_x = 0.0;
-      accel_y = 0.0;
-      accel_z = 0.0;
-      mag_x = 0.0;
-      mag_y = 0.0;
-      mag_z = 0.0;
-      baro = 0.0;
-      temp = 0.0;
-      gyro_rate_x_si = 0.0;
-      gyro_rate_y_si = 0.0;
-      // gyro_rate_z_si = 0.0;
-      accel_x_si = 0.0;
-      accel_y_si = 0.0;
-      accel_z_si = 0.0;
       compAngleX = 0.0;
       compAngleY = 0.0;
-      accelAngleX = 0.0;
-      accelAngleY = 0.0;
     }
   }
 }
@@ -826,16 +756,6 @@ double ADIS16448_IMU::FormatFastConverge(double compAngle, double accAngle) {
   return compAngle;
 }
 
-double ADIS16448_IMU::FormatRange0to2PI(double compAngle) {
-  while (compAngle >= 2 * std::numbers::pi) {
-    compAngle = compAngle - 2.0 * std::numbers::pi;
-  }
-  while (compAngle < 0.0) {
-    compAngle = compAngle + 2.0 * std::numbers::pi;
-  }
-  return compAngle;
-}
-
 double ADIS16448_IMU::FormatAccelRange(double accelAngle, double accelZ) {
   if (accelZ < 0.0) {
     accelAngle = std::numbers::pi - accelAngle;
@@ -850,43 +770,35 @@ double ADIS16448_IMU::CompFilterProcess(double compAngle, double accelAngle,
   compAngle = FormatFastConverge(compAngle, accelAngle);
   compAngle =
       m_alpha * (compAngle + omega * m_dt) + (1.0 - m_alpha) * accelAngle;
-  compAngle = FormatRange0to2PI(compAngle);
-  if (compAngle > std::numbers::pi) {
-    compAngle = compAngle - 2.0 * std::numbers::pi;
-  }
-  return compAngle;
+  return frc::InputModulus(compAngle, -std::numbers::pi, std::numbers::pi);
 }
 
 int ADIS16448_IMU::ConfigDecRate(uint16_t decimationRate) {
   // Switches the active SPI port to standard SPI mode, writes a new value to
-  // the DECIMATE register in the IMU, and re-enables auto SPI.
-  //
-  // This function enters standard SPI mode, writes a new DECIMATE setting to
-  // the IMU, adjusts the sample scale factor, and re-enters auto SPI mode.
-  uint16_t writeValue = decimationRate;
-  uint16_t readbackValue;
+  // the DECIMATE register in the IMU, adjusts the sample scale factor, and
+  // re-enables auto SPI.
   if (!SwitchToStandardSPI()) {
     REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
     return 2;
   }
 
-  /* Check max */
+  // Check max
   if (decimationRate > 9) {
     REPORT_ERROR(
         "Attempted to write an invalid decimation value. Capping at 9");
     decimationRate = 9;
   }
 
-  /* Shift decimation setting to correct position and select internal sync */
-  writeValue = (decimationRate << 8) | 0x1;
+  // Shift decimation setting to correct position and select internal sync
+  uint16_t writeValue = (decimationRate << 8) | 0x1;
 
-  /* Apply to IMU */
+  // Apply to IMU
   WriteRegister(SMPL_PRD, writeValue);
 
-  /* Perform read back to verify write */
-  readbackValue = ReadRegister(SMPL_PRD);
+  // Perform read back to verify write
+  uint16_t readbackValue = ReadRegister(SMPL_PRD);
 
-  /* Throw error for invalid write */
+  // Throw error for invalid write
   if (readbackValue != writeValue) {
     REPORT_ERROR("ADIS16448 SMPL_PRD write failed.");
   }
@@ -1059,12 +971,6 @@ int ADIS16448_IMU::GetPort() const {
   return m_spi_port;
 }
 
-/**
- * @brief Builds a Sendable object to push IMU data to the driver station.
- *
- * This function pushes the most recent angle estimates for all axes to the
- *driver station.
- **/
 void ADIS16448_IMU::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("ADIS16448 IMU");
   builder.AddDoubleProperty(
diff --git a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp
index 0f8da84a21..1ff56d27b8 100644
--- a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp
+++ b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp
@@ -27,6 +27,7 @@
 #include <wpi/sendable/SendableRegistry.h>
 
 #include "frc/Errors.h"
+#include "frc/MathUtil.h"
 
 /* Helpful conversion functions */
 static inline int32_t ToInt(const uint32_t* buf) {
@@ -38,10 +39,6 @@ static inline int16_t BuffToShort(const uint32_t* buf) {
   return (static_cast<int16_t>(buf[0]) << 8) | buf[1];
 }
 
-static inline uint16_t ToUShort(const uint8_t* buf) {
-  return (static_cast<uint16_t>(buf[0]) << 8) | buf[1];
-}
-
 using namespace frc;
 
 namespace {
@@ -120,35 +117,33 @@ ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis,
     // Relies on the RIO hardware by default configuring an output as low
     // and configuring an input as high Z. The 10k pull-up resistor internal to
     // the IMU then forces the reset line high for normal operation.
-    DigitalOutput* m_reset_out =
+    DigitalOutput* reset_out =
         new DigitalOutput(27);  // Drive SPI CS2 (IMU RST) low
-    Wait(10_ms);                // Wait 10ms
-    delete m_reset_out;
+    Wait(10_ms);
+    delete reset_out;
     m_reset_in = new DigitalInput(27);  // Set SPI CS2 (IMU RST) high
-    Wait(500_ms);                       // Wait 500ms for reset to complete
+    Wait(500_ms);                       // Wait for reset to complete
 
     // Configure standard SPI
     if (!SwitchToStandardSPI()) {
       return;
     }
-
-    // Set up flash state variable
-    bool m_needs_flash = false;
-
+    bool needsFlash = false;
     // Set IMU internal decimation to 4 (output data rate of 2000 SPS / (4 + 1)
     // = 400Hz)
     if (ReadRegister(DEC_RATE) != 0x0004) {
       WriteRegister(DEC_RATE, 0x0004);
-      m_needs_flash = true;
+      needsFlash = true;
       REPORT_WARNING(
           "ADIS16470: DEC_RATE register configuration inconsistent! Scheduling "
           "flash update.");
     }
+
     // Set data ready polarity (HIGH = Good Data), Disable gSense Compensation
     // and PoP
     if (ReadRegister(MSC_CTRL) != 0x0001) {
       WriteRegister(MSC_CTRL, 0x0001);
-      m_needs_flash = true;
+      needsFlash = true;
       REPORT_WARNING(
           "ADIS16470: MSC_CTRL register configuration inconsistent! Scheduling "
           "flash update.");
@@ -157,7 +152,7 @@ ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis,
     // Disable IMU internal Bartlett filter (200Hz bandwidth is sufficient)
     if (ReadRegister(FILT_CTRL) != 0x0000) {
       WriteRegister(FILT_CTRL, 0x0000);
-      m_needs_flash = true;
+      needsFlash = true;
       REPORT_WARNING(
           "ADIS16470: FILT_CTRL register configuration inconsistent! "
           "Scheduling flash update.");
@@ -165,7 +160,7 @@ ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis,
 
     // If any registers on the IMU don't match the config, trigger a flash
     // update
-    if (m_needs_flash) {
+    if (needsFlash) {
       REPORT_WARNING(
           "ADIS16470: Register configuration changed! Starting IMU flash "
           "update.");
@@ -174,7 +169,6 @@ ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis,
       // the datasheet)
       Wait(0.3_s);
       REPORT_WARNING("ADIS16470: Flash update finished!");
-      m_needs_flash = false;
     } else {
       REPORT_WARNING(
           "ADIS16470: Flash and RAM configuration consistent. No flash update "
@@ -199,7 +193,7 @@ ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis,
       return;
     }
 
-    // Let the user know the IMU was initiallized successfully
+    // Let the user know the IMU was initialized successfully
     REPORT_WARNING("ADIS16470 IMU Successfully Initialized!");
 
     // Drive SPI CS3 (IMU ready LED) low (active low)
@@ -228,7 +222,6 @@ ADIS16470_IMU::ADIS16470_IMU(ADIS16470_IMU&& other)
       m_accel_x{std::move(other.m_accel_x)},
       m_accel_y{std::move(other.m_accel_y)},
       m_accel_z{std::move(other.m_accel_z)},
-      m_tau{std::move(other.m_tau)},
       m_dt{std::move(other.m_dt)},
       m_alpha{std::move(other.m_alpha)},
       m_compAngleX{std::move(other.m_compAngleX)},
@@ -278,7 +271,6 @@ ADIS16470_IMU& ADIS16470_IMU::operator=(ADIS16470_IMU&& other) {
   std::swap(this->m_accel_x, other.m_accel_x);
   std::swap(this->m_accel_y, other.m_accel_y);
   std::swap(this->m_accel_z, other.m_accel_z);
-  std::swap(this->m_tau, other.m_tau);
   std::swap(this->m_dt, other.m_dt);
   std::swap(this->m_alpha, other.m_alpha);
   std::swap(this->m_compAngleX, other.m_compAngleX);
@@ -343,18 +335,17 @@ bool ADIS16470_IMU::SwitchToStandardSPI() {
     if (m_spi != nullptr && m_auto_configured) {
       m_spi->StopAuto();
       // We need to get rid of all the garbage left in the auto SPI buffer after
-      // stopping it. Sometimes data magically reappears, so we have to check
-      // the buffer size a couple of times
-      //  to be sure we got it all. Yuck.
+      // stopping it.
+      // Sometimes data magically reappears, so we have to check the buffer size
+      // a couple of times to be sure we got it all. Yuck.
       uint32_t trashBuffer[200];
       Wait(100_ms);
       int data_count = m_spi->ReadAutoReceivedData(trashBuffer, 0, 0_s);
       while (data_count > 0) {
-        /* Receive data, max of 200 words at a time (prevent potential segfault)
-         */
+        // Receive data, max of 200 words at a time (prevent potential segfault)
         m_spi->ReadAutoReceivedData(trashBuffer, (std::min)(data_count, 200),
                                     0_s);
-        /*Get the remaining data count */
+        // Get the remaining data count
         data_count = m_spi->ReadAutoReceivedData(trashBuffer, 0, 0_s);
       }
     }
@@ -408,12 +399,11 @@ bool ADIS16470_IMU::SwitchToStandardSPI() {
  **/
 bool ADIS16470_IMU::SwitchToAutoSPI() {
   // No SPI port has been set up. Go set one up first.
-  if (m_spi == nullptr) {
-    if (!SwitchToStandardSPI()) {
-      REPORT_ERROR("Failed to start/restart auto SPI");
-      return false;
-    }
+  if (m_spi == nullptr && !SwitchToStandardSPI()) {
+    REPORT_ERROR("Failed to start/restart auto SPI");
+    return false;
   }
+
   // Only set up the interrupt if needed.
   if (m_auto_interrupt == nullptr) {
     m_auto_interrupt = new DigitalInput(26);
@@ -426,12 +416,11 @@ bool ADIS16470_IMU::SwitchToAutoSPI() {
   }
   // Do we need to change auto SPI settings?
   m_spi->SetAutoTransmitData(m_autospi_allangle_packet, 2);
-
   // Configure auto stall time
   m_spi->ConfigureAutoStall(HAL_SPI_kOnboardCS0, 5, 1000, 1);
   // Kick off DMA SPI (Note: Device configuration impossible after SPI DMA is
-  // activated) DR High = Data good (data capture should be triggered on the
-  // rising edge)
+  // activated)
+  // DR High = Data good (data capture should be triggered on the rising edge)
   m_spi->StartAutoTrigger(*m_auto_interrupt, true, false);
   // Check to see if the acquire thread is running. If not, kick one off.
   if (!m_thread_idle) {
@@ -488,10 +477,8 @@ int ADIS16470_IMU::ConfigCalTime(CalibrationTime new_cal_time) {
 
 int ADIS16470_IMU::ConfigDecRate(uint16_t decimationRate) {
   // Switches the active SPI port to standard SPI mode, writes a new value to
-  // the DECIMATE register in the IMU, and re-enables auto SPI.
-  //
-  // This function enters standard SPI mode, writes a new DECIMATE setting to
-  // the IMU, adjusts the sample scale factor, and re-enters auto SPI mode.
+  // the DECIMATE register in the IMU, adjusts the sample scale factor, and
+  // re-enables auto SPI.
   if (!SwitchToStandardSPI()) {
     REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
     return 2;
@@ -500,7 +487,7 @@ int ADIS16470_IMU::ConfigDecRate(uint16_t decimationRate) {
     REPORT_ERROR("Attempted to write an invalid decimation value.");
     decimationRate = 1999;
   }
-  m_scaled_sample_rate = (((decimationRate + 1.0) / 2000.0) * 1000000.0);
+  m_scaled_sample_rate = (decimationRate + 1.0) / 2000.0 * 1000000.0;
   WriteRegister(DEC_RATE, decimationRate);
   if (!SwitchToAutoSPI()) {
     REPORT_ERROR("Failed to configure/reconfigure auto SPI.");
@@ -550,24 +537,24 @@ uint16_t ADIS16470_IMU::ReadRegister(uint8_t reg) {
   m_spi->Write(buf, 2);
   m_spi->Read(false, buf, 2);
 
-  return ToUShort(buf);
+  return (static_cast<uint16_t>(buf[0]) << 8) | buf[1];
 }
 
 /**
  * @brief Writes an unsigned, 16-bit value to two adjacent, 8-bit register
- *locations over SPI.
+ * locations over SPI.
  *
  * @param reg An unsigned, 8-bit register location.
  *
  * @param val An unsigned, 16-bit value to be written to the specified register
- *location.
+ * location.
  *
  * This function writes an unsigned, 16-bit value into adjacent 8-bit addresses
- *via SPI. The upper and lower bytes that make up the 16-bit value are split
- *into two unsined, 8-bit values and written to the upper and lower addresses of
- *the specified register value. Only the lower (base) address must be specified.
- *This function assumes the controller is set to standard SPI mode.
- **/
+ * via SPI. The upper and lower bytes that make up the 16-bit value are split
+ * into two unsigned, 8-bit values and written to the upper and lower addresses
+ * of the specified register value. Only the lower (base) address must be
+ * specified. This function assumes the controller is set to standard SPI mode.
+ */
 void ADIS16470_IMU::WriteRegister(uint8_t reg, uint16_t val) {
   uint8_t buf[2];
   buf[0] = 0x80 | reg;
@@ -646,110 +633,81 @@ ADIS16470_IMU::~ADIS16470_IMU() {
 void ADIS16470_IMU::Acquire() {
   // Set data packet length
   const int dataset_len = 27;  // 26 data points + timestamp
-
-  /* Fixed buffer size */
   const int BUFFER_SIZE = 4000;
 
   // This buffer can contain many datasets
   uint32_t buffer[BUFFER_SIZE];
-  int data_count = 0;
-  int data_remainder = 0;
-  int data_to_read = 0;
   uint32_t previous_timestamp = 0;
-  double delta_angle_x = 0.0;
-  double delta_angle_y = 0.0;
-  double delta_angle_z = 0.0;
-  double gyro_rate_x = 0.0;
-  double gyro_rate_y = 0.0;
-  double gyro_rate_z = 0.0;
-  double accel_x = 0.0;
-  double accel_y = 0.0;
-  double accel_z = 0.0;
-  double gyro_rate_x_si = 0.0;
-  double gyro_rate_y_si = 0.0;
-  // double gyro_rate_z_si = 0.0;
-  double accel_x_si = 0.0;
-  double accel_y_si = 0.0;
-  double accel_z_si = 0.0;
   double compAngleX = 0.0;
   double compAngleY = 0.0;
-  double accelAngleX = 0.0;
-  double accelAngleY = 0.0;
-
   while (true) {
-    // Sleep loop for 10ms (wait for data)
+    // Wait for data
     Wait(10_ms);
 
     if (m_thread_active) {
       m_thread_idle = false;
 
-      data_count = m_spi->ReadAutoReceivedData(
-          buffer, 0,
-          0_s);  // Read number of bytes currently stored in the buffer
-      data_remainder =
-          data_count % dataset_len;  // Check if frame is incomplete. Add 1
-                                     // because of timestamp
-      data_to_read = data_count -
-                     data_remainder;  // Remove incomplete data from read count
-      /* Want to cap the data to read in a single read at the buffer size */
+      // Read number of bytes currently stored in the buffer
+      int data_count = m_spi->ReadAutoReceivedData(buffer, 0, 0_s);
+      // Check if frame is incomplete. Add 1 because of timestamp
+      int data_remainder = data_count % dataset_len;
+      // Remove incomplete data from read count
+      int data_to_read = data_count - data_remainder;
+      // Want to cap the data to read in a single read at the buffer size
       if (data_to_read > BUFFER_SIZE) {
         REPORT_WARNING(
             "ADIS16470 data processing thread overrun has occurred!");
         data_to_read = BUFFER_SIZE - (BUFFER_SIZE % dataset_len);
       }
-      m_spi->ReadAutoReceivedData(
-          buffer, data_to_read,
-          0_s);  // Read data from DMA buffer (only complete sets)
+      // Read data from DMA buffer (only complete sets)
+      m_spi->ReadAutoReceivedData(buffer, data_to_read, 0_s);
 
       // Could be multiple data sets in the buffer. Handle each one.
       for (int i = 0; i < data_to_read; i += dataset_len) {
         // Timestamp is at buffer[i]
         m_dt = (buffer[i] - previous_timestamp) / 1000000.0;
-        /* Get delta angle value for selected yaw axis and scale by the elapsed
-         * time (based on timestamp) */
-        delta_angle_x =
-            (ToInt(&buffer[i + 3]) * delta_angle_sf) /
-            (m_scaled_sample_rate / (buffer[i] - previous_timestamp));
-        delta_angle_y =
-            (ToInt(&buffer[i + 7]) * delta_angle_sf) /
-            (m_scaled_sample_rate / (buffer[i] - previous_timestamp));
-        delta_angle_z =
-            (ToInt(&buffer[i + 11]) * delta_angle_sf) /
-            (m_scaled_sample_rate / (buffer[i] - previous_timestamp));
+        // Get delta angle value for selected yaw axis and scale by the elapsed
+        // time (based on timestamp)
+        double elapsed_time =
+            m_scaled_sample_rate / (buffer[i] - previous_timestamp);
+        double delta_angle_x =
+            ToInt(&buffer[i + 3]) * delta_angle_sf / elapsed_time;
+        double delta_angle_y =
+            ToInt(&buffer[i + 7]) * delta_angle_sf / elapsed_time;
+        double delta_angle_z =
+            ToInt(&buffer[i + 11]) * delta_angle_sf / elapsed_time;
 
-        gyro_rate_x = (BuffToShort(&buffer[i + 15]) / 10.0);
-        gyro_rate_y = (BuffToShort(&buffer[i + 17]) / 10.0);
-        gyro_rate_z = (BuffToShort(&buffer[i + 19]) / 10.0);
-        accel_x = (BuffToShort(&buffer[i + 21]) / 800.0);
-        accel_y = (BuffToShort(&buffer[i + 23]) / 800.0);
-        accel_z = (BuffToShort(&buffer[i + 25]) / 800.0);
+        double gyro_rate_x = BuffToShort(&buffer[i + 15]) / 10.0;
+        double gyro_rate_y = BuffToShort(&buffer[i + 17]) / 10.0;
+        double gyro_rate_z = BuffToShort(&buffer[i + 19]) / 10.0;
+        double accel_x = BuffToShort(&buffer[i + 21]) / 800.0;
+        double accel_y = BuffToShort(&buffer[i + 23]) / 800.0;
+        double accel_z = BuffToShort(&buffer[i + 25]) / 800.0;
 
         // Convert scaled sensor data to SI units
-        gyro_rate_x_si = gyro_rate_x * deg_to_rad;
-        gyro_rate_y_si = gyro_rate_y * deg_to_rad;
-        // gyro_rate_z_si = gyro_rate_z * deg_to_rad;
-        accel_x_si = accel_x * grav;
-        accel_y_si = accel_y * grav;
-        accel_z_si = accel_z * grav;
+        double gyro_rate_x_si = gyro_rate_x * deg_to_rad;
+        double gyro_rate_y_si = gyro_rate_y * deg_to_rad;
+        // double gyro_rate_z_si = gyro_rate_z * deg_to_rad;
+        double accel_x_si = accel_x * grav;
+        double accel_y_si = accel_y * grav;
+        double accel_z_si = accel_z * grav;
 
         // Store timestamp for next iteration
         previous_timestamp = buffer[i];
 
-        m_alpha = m_tau / (m_tau + m_dt);
+        m_alpha = 1.0 / (1.0 + m_dt);
 
+        // Run inclinometer calculations
+        double accelAngleX = atan2f(
+            accel_x_si,
+            sqrtf((accel_y_si * accel_y_si) + (accel_z_si * accel_z_si)));
+        double accelAngleY = atan2f(
+            accel_y_si,
+            sqrtf((accel_x_si * accel_x_si) + (accel_z_si * accel_z_si)));
         if (m_first_run) {
-          accelAngleX = atan2f(accel_x_si, sqrtf((accel_y_si * accel_y_si) +
-                                                 (accel_z_si * accel_z_si)));
-          accelAngleY = atan2f(accel_y_si, sqrtf((accel_x_si * accel_x_si) +
-                                                 (accel_z_si * accel_z_si)));
           compAngleX = accelAngleX;
           compAngleY = accelAngleY;
         } else {
-          // Process X angle
-          accelAngleX = atan2f(accel_x_si, sqrtf((accel_y_si * accel_y_si) +
-                                                 (accel_z_si * accel_z_si)));
-          accelAngleY = atan2f(accel_y_si, sqrtf((accel_x_si * accel_x_si) +
-                                                 (accel_z_si * accel_z_si)));
           accelAngleX = FormatAccelRange(accelAngleX, accel_z_si);
           accelAngleY = FormatAccelRange(accelAngleY, accel_z_si);
           compAngleX =
@@ -760,10 +718,10 @@ void ADIS16470_IMU::Acquire() {
 
         {
           std::scoped_lock sync(m_mutex);
-          /* Push data to global variables */
+          // Push data to global variables
           if (m_first_run) {
-            /* Don't accumulate first run. previous_timestamp will be "very" old
-             * and the integration will end up way off */
+            // Don't accumulate first run. previous_timestamp will be "very" old
+            // and the integration will end up way off
             m_integ_angle_x = 0.0;
             m_integ_angle_y = 0.0;
             m_integ_angle_z = 0.0;
@@ -787,29 +745,9 @@ void ADIS16470_IMU::Acquire() {
       }
     } else {
       m_thread_idle = true;
-      data_count = 0;
-      data_remainder = 0;
-      data_to_read = 0;
-      previous_timestamp = 0.0;
-      delta_angle_x = 0.0;
-      delta_angle_y = 0.0;
-      delta_angle_z = 0.0;
-      gyro_rate_x = 0.0;
-      gyro_rate_y = 0.0;
-      gyro_rate_z = 0.0;
-      accel_x = 0.0;
-      accel_y = 0.0;
-      accel_z = 0.0;
-      gyro_rate_x_si = 0.0;
-      gyro_rate_y_si = 0.0;
-      // gyro_rate_z_si = 0.0;
-      accel_x_si = 0.0;
-      accel_y_si = 0.0;
-      accel_z_si = 0.0;
+      previous_timestamp = 0;
       compAngleX = 0.0;
       compAngleY = 0.0;
-      accelAngleX = 0.0;
-      accelAngleY = 0.0;
     }
   }
 }
@@ -824,16 +762,6 @@ double ADIS16470_IMU::FormatFastConverge(double compAngle, double accAngle) {
   return compAngle;
 }
 
-double ADIS16470_IMU::FormatRange0to2PI(double compAngle) {
-  while (compAngle >= 2 * std::numbers::pi) {
-    compAngle = compAngle - 2.0 * std::numbers::pi;
-  }
-  while (compAngle < 0.0) {
-    compAngle = compAngle + 2.0 * std::numbers::pi;
-  }
-  return compAngle;
-}
-
 double ADIS16470_IMU::FormatAccelRange(double accelAngle, double accelZ) {
   if (accelZ < 0.0) {
     accelAngle = std::numbers::pi - accelAngle;
@@ -848,11 +776,7 @@ double ADIS16470_IMU::CompFilterProcess(double compAngle, double accelAngle,
   compAngle = FormatFastConverge(compAngle, accelAngle);
   compAngle =
       m_alpha * (compAngle + omega * m_dt) + (1.0 - m_alpha) * accelAngle;
-  compAngle = FormatRange0to2PI(compAngle);
-  if (compAngle > std::numbers::pi) {
-    compAngle = compAngle - 2.0 * std::numbers::pi;
-  }
-  return compAngle;
+  return frc::InputModulus(compAngle, -std::numbers::pi, std::numbers::pi);
 }
 
 void ADIS16470_IMU::SetGyroAngle(IMUAxis axis, units::degree_t angle) {
@@ -1054,12 +978,6 @@ int ADIS16470_IMU::GetPort() const {
   return m_spi_port;
 }
 
-/**
- * @brief Builds a Sendable object to push IMU data to the driver station.
- *
- * This function pushes the most recent angle estimates for all axes to the
- *driver station.
- **/
 void ADIS16470_IMU::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("ADIS16470 IMU");
   builder.AddDoubleProperty(
diff --git a/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h b/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h
index 0b16782b8a..44b17d7ec5 100644
--- a/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h
+++ b/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h
@@ -281,7 +281,7 @@ class ADIS16448_IMU : public wpi::Sendable,
   void InitSendable(wpi::SendableBuilder& builder) override;
 
  private:
-  /** @brief ADIS16448 Register Map Declaration */
+  // ADIS16448 Register Map Declaration
   static constexpr uint8_t FLASH_CNT = 0x00;  // Flash memory write count
   static constexpr uint8_t XGYRO_OUT = 0x04;  // X-axis gyroscope output
   static constexpr uint8_t YGYRO_OUT = 0x06;  // Y-axis gyroscope output
@@ -382,7 +382,6 @@ class ADIS16448_IMU : public wpi::Sendable,
   double m_temp = 0.0;
 
   // Complementary filter variables
-  double m_tau = 0.5;
   double m_dt, m_alpha = 0.0;
   double m_compAngleX, m_compAngleY, m_accelAngleX, m_accelAngleY = 0.0;
 
@@ -408,8 +407,6 @@ class ADIS16448_IMU : public wpi::Sendable,
   // Complementary filter functions
   double FormatFastConverge(double compAngle, double accAngle);
 
-  double FormatRange0to2PI(double compAngle);
-
   double FormatAccelRange(double accelAngle, double accelZ);
 
   double CompFilterProcess(double compAngle, double accelAngle, double omega);
@@ -463,7 +460,7 @@ class ADIS16448_IMU : public wpi::Sendable,
   mutable NonMovableMutexWrapper m_mutex;
 
   // CRC-16 Look-Up Table
-  static constexpr uint16_t adiscrc[256] = {
+  static constexpr uint16_t m_adiscrc[256] = {
       0x0000, 0x17CE, 0x0FDF, 0x1811, 0x1FBE, 0x0870, 0x1061, 0x07AF, 0x1F3F,
       0x08F1, 0x10E0, 0x072E, 0x0081, 0x174F, 0x0F5E, 0x1890, 0x1E3D, 0x09F3,
       0x11E2, 0x062C, 0x0183, 0x164D, 0x0E5C, 0x1992, 0x0102, 0x16CC, 0x0EDD,
diff --git a/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h
index f4f4145e17..bdb164fd90 100644
--- a/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h
+++ b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h
@@ -162,8 +162,10 @@ class ADIS16470_IMU : public wpi::Sendable,
   void Calibrate();
 
   /**
-   * @brief Switches the active SPI port to standard SPI mode, writes a new
-   * value to the NULL_CNFG register in the IMU, and re-enables auto SPI.
+   * Configures calibration time.
+   *
+   * @param new_cal_time New calibration time
+   * @return 0 if success, 1 if no change, 2 if error.
    */
   int ConfigCalTime(CalibrationTime new_cal_time);
 
@@ -422,7 +424,7 @@ class ADIS16470_IMU : public wpi::Sendable,
   static constexpr double deg_to_rad = 0.0174532;
   static constexpr double grav = 9.81;
 
-  /** @brief Resources **/
+  // Resources
   DigitalInput* m_reset_in = nullptr;
   DigitalOutput* m_status_led = nullptr;
 
@@ -487,15 +489,12 @@ class ADIS16470_IMU : public wpi::Sendable,
   double m_accel_z = 0.0;
 
   // Complementary filter variables
-  double m_tau = 1.0;
   double m_dt, m_alpha = 0.0;
   double m_compAngleX, m_compAngleY, m_accelAngleX, m_accelAngleY = 0.0;
 
   // Complementary filter functions
   double FormatFastConverge(double compAngle, double accAngle);
 
-  double FormatRange0to2PI(double compAngle);
-
   double FormatAccelRange(double accelAngle, double accelZ);
 
   double CompFilterProcess(double compAngle, double accelAngle, double omega);
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java
index 1d79d0d669..8eeae4a279 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java
@@ -18,40 +18,20 @@ import edu.wpi.first.hal.HAL;
 import edu.wpi.first.hal.SimBoolean;
 import edu.wpi.first.hal.SimDevice;
 import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.util.sendable.Sendable;
 import edu.wpi.first.util.sendable.SendableBuilder;
 
 // CHECKSTYLE.OFF: TypeName
 // CHECKSTYLE.OFF: MemberName
-// CHECKSTYLE.OFF: SummaryJavadoc
-// CHECKSTYLE.OFF: UnnecessaryParentheses
-// CHECKSTYLE.OFF: OverloadMethodsDeclarationOrder
-// CHECKSTYLE.OFF: NonEmptyAtclauseDescription
-// CHECKSTYLE.OFF: MissingOverride
-// CHECKSTYLE.OFF: AtclauseOrder
 // CHECKSTYLE.OFF: LocalVariableName
-// CHECKSTYLE.OFF: RedundantModifier
-// CHECKSTYLE.OFF: AbbreviationAsWordInName
 // CHECKSTYLE.OFF: ParameterName
 // CHECKSTYLE.OFF: EmptyCatchBlock
-// CHECKSTYLE.OFF: MissingJavadocMethod
-// CHECKSTYLE.OFF: MissingSwitchDefault
-// CHECKSTYLE.OFF: VariableDeclarationUsageDistance
-// CHECKSTYLE.OFF: ArrayTypeStyle
 
 /** This class is for the ADIS16448 IMU that connects to the RoboRIO MXP port. */
-@SuppressWarnings({
-  "unused",
-  "PMD.RedundantFieldInitializer",
-  "PMD.ImmutableField",
-  "PMD.SingularField",
-  "PMD.CollapsibleIfStatements",
-  "PMD.MissingOverride",
-  "PMD.EmptyIfStmt",
-  "PMD.EmptyStatementNotInLoop"
-})
+@SuppressWarnings({"PMD.RedundantFieldInitializer", "PMD.ImmutableField"})
 public class ADIS16448_IMU implements AutoCloseable, Sendable {
-  /** ADIS16448 Register Map Declaration */
+  // ADIS16448 Register Map Declaration
   private static final int FLASH_CNT = 0x00; // Flash memory write count
 
   private static final int XGYRO_OUT = 0x04; // X-axis gyroscope output
@@ -96,29 +76,29 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
 
   /** ADIS16448 calibration times. */
   public enum CalibrationTime {
-    /** 32 ms calibration time */
+    /** 32 ms calibration time. */
     _32ms(0),
-    /** 64 ms calibration time */
+    /** 64 ms calibration time. */
     _64ms(1),
-    /** 128 ms calibration time */
+    /** 128 ms calibration time. */
     _128ms(2),
-    /** 256 ms calibration time */
+    /** 256 ms calibration time. */
     _256ms(3),
-    /** 512 ms calibration time */
+    /** 512 ms calibration time. */
     _512ms(4),
-    /** 1 s calibration time */
+    /** 1 s calibration time. */
     _1s(5),
-    /** 2 s calibration time */
+    /** 2 s calibration time. */
     _2s(6),
-    /** 4 s calibration time */
+    /** 4 s calibration time. */
     _4s(7),
-    /** 8 s calibration time */
+    /** 8 s calibration time. */
     _8s(8),
-    /** 16 s calibration time */
+    /** 16 s calibration time. */
     _16s(9),
-    /** 32 s calibration time */
+    /** 32 s calibration time. */
     _32s(10),
-    /** 64 s calibration time */
+    /** 64 s calibration time. */
     _64s(11);
 
     private final int value;
@@ -138,20 +118,17 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
     kZ
   }
 
-  // * Static Constants */
-  private static final double rad_to_deg = 57.2957795;
-  private static final double deg_to_rad = 0.0174532;
   private static final double grav = 9.81;
 
-  /* User-specified yaw axis */
+  // User-specified yaw axis
   private IMUAxis m_yaw_axis;
 
-  /* Offset data storage */
+  // Offset data storage
   private double[] m_offset_data_gyro_rate_x;
   private double[] m_offset_data_gyro_rate_y;
   private double[] m_offset_data_gyro_rate_z;
 
-  /* Instant raw output variables */
+  // Instant raw output variables
   private double m_gyro_rate_x = 0.0;
   private double m_gyro_rate_y = 0.0;
   private double m_gyro_rate_z = 0.0;
@@ -164,41 +141,38 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
   private double m_baro = 0.0;
   private double m_temp = 0.0;
 
-  /* IMU gyro offset variables */
+  // IMU gyro offset variables
   private double m_gyro_rate_offset_x = 0.0;
   private double m_gyro_rate_offset_y = 0.0;
   private double m_gyro_rate_offset_z = 0.0;
   private int m_avg_size = 0;
   private int m_accum_count = 0;
 
-  /* Integrated gyro angle variables */
+  // Integrated gyro angle variables
   private double m_integ_gyro_angle_x = 0.0;
   private double m_integ_gyro_angle_y = 0.0;
   private double m_integ_gyro_angle_z = 0.0;
 
-  /* Complementary filter variables */
+  // Complementary filter variables
   private double m_dt = 0.0;
   private double m_alpha = 0.0;
-  private double m_tau = 1.0;
   private double m_compAngleX = 0.0;
   private double m_compAngleY = 0.0;
   private double m_accelAngleX = 0.0;
   private double m_accelAngleY = 0.0;
 
-  /* State variables */
+  // State variables
   private volatile boolean m_thread_active = false;
   private CalibrationTime m_calibration_time = CalibrationTime._32ms;
   private volatile boolean m_first_run = true;
   private volatile boolean m_thread_idle = false;
   private boolean m_auto_configured = false;
   private boolean m_start_up_mode = true;
-  private boolean m_needs_flash = false;
 
-  /* Resources */
+  // Resources
   private SPI m_spi;
   private SPI.Port m_spi_port;
   private DigitalInput m_auto_interrupt;
-  private DigitalOutput m_reset_out;
   private DigitalInput m_reset_in;
   private DigitalOutput m_status_led;
   private Thread m_acquire_task;
@@ -216,55 +190,41 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
   private SimDouble m_simAccelY;
   private SimDouble m_simAccelZ;
 
-  /* CRC-16 Look-Up Table */
-  int[] adiscrc =
-      new int[] {
-        0x0000, 0x17CE, 0x0FDF, 0x1811, 0x1FBE, 0x0870, 0x1061, 0x07AF,
-        0x1F3F, 0x08F1, 0x10E0, 0x072E, 0x0081, 0x174F, 0x0F5E, 0x1890,
-        0x1E3D, 0x09F3, 0x11E2, 0x062C, 0x0183, 0x164D, 0x0E5C, 0x1992,
-        0x0102, 0x16CC, 0x0EDD, 0x1913, 0x1EBC, 0x0972, 0x1163, 0x06AD,
-        0x1C39, 0x0BF7, 0x13E6, 0x0428, 0x0387, 0x1449, 0x0C58, 0x1B96,
-        0x0306, 0x14C8, 0x0CD9, 0x1B17, 0x1CB8, 0x0B76, 0x1367, 0x04A9,
-        0x0204, 0x15CA, 0x0DDB, 0x1A15, 0x1DBA, 0x0A74, 0x1265, 0x05AB,
-        0x1D3B, 0x0AF5, 0x12E4, 0x052A, 0x0285, 0x154B, 0x0D5A, 0x1A94,
-        0x1831, 0x0FFF, 0x17EE, 0x0020, 0x078F, 0x1041, 0x0850, 0x1F9E,
-        0x070E, 0x10C0, 0x08D1, 0x1F1F, 0x18B0, 0x0F7E, 0x176F, 0x00A1,
-        0x060C, 0x11C2, 0x09D3, 0x1E1D, 0x19B2, 0x0E7C, 0x166D, 0x01A3,
-        0x1933, 0x0EFD, 0x16EC, 0x0122, 0x068D, 0x1143, 0x0952, 0x1E9C,
-        0x0408, 0x13C6, 0x0BD7, 0x1C19, 0x1BB6, 0x0C78, 0x1469, 0x03A7,
-        0x1B37, 0x0CF9, 0x14E8, 0x0326, 0x0489, 0x1347, 0x0B56, 0x1C98,
-        0x1A35, 0x0DFB, 0x15EA, 0x0224, 0x058B, 0x1245, 0x0A54, 0x1D9A,
-        0x050A, 0x12C4, 0x0AD5, 0x1D1B, 0x1AB4, 0x0D7A, 0x156B, 0x02A5,
-        0x1021, 0x07EF, 0x1FFE, 0x0830, 0x0F9F, 0x1851, 0x0040, 0x178E,
-        0x0F1E, 0x18D0, 0x00C1, 0x170F, 0x10A0, 0x076E, 0x1F7F, 0x08B1,
-        0x0E1C, 0x19D2, 0x01C3, 0x160D, 0x11A2, 0x066C, 0x1E7D, 0x09B3,
-        0x1123, 0x06ED, 0x1EFC, 0x0932, 0x0E9D, 0x1953, 0x0142, 0x168C,
-        0x0C18, 0x1BD6, 0x03C7, 0x1409, 0x13A6, 0x0468, 0x1C79, 0x0BB7,
-        0x1327, 0x04E9, 0x1CF8, 0x0B36, 0x0C99, 0x1B57, 0x0346, 0x1488,
-        0x1225, 0x05EB, 0x1DFA, 0x0A34, 0x0D9B, 0x1A55, 0x0244, 0x158A,
-        0x0D1A, 0x1AD4, 0x02C5, 0x150B, 0x12A4, 0x056A, 0x1D7B, 0x0AB5,
-        0x0810, 0x1FDE, 0x07CF, 0x1001, 0x17AE, 0x0060, 0x1871, 0x0FBF,
-        0x172F, 0x00E1, 0x18F0, 0x0F3E, 0x0891, 0x1F5F, 0x074E, 0x1080,
-        0x162D, 0x01E3, 0x19F2, 0x0E3C, 0x0993, 0x1E5D, 0x064C, 0x1182,
-        0x0912, 0x1EDC, 0x06CD, 0x1103, 0x16AC, 0x0162, 0x1973, 0x0EBD,
-        0x1429, 0x03E7, 0x1BF6, 0x0C38, 0x0B97, 0x1C59, 0x0448, 0x1386,
-        0x0B16, 0x1CD8, 0x04C9, 0x1307, 0x14A8, 0x0366, 0x1B77, 0x0CB9,
-        0x0A14, 0x1DDA, 0x05CB, 0x1205, 0x15AA, 0x0264, 0x1A75, 0x0DBB,
-        0x152B, 0x02E5, 0x1AF4, 0x0D3A, 0x0A95, 0x1D5B, 0x054A, 0x1284
-      };
-
-  private static class AcquireTask implements Runnable {
-    private final ADIS16448_IMU imu;
-
-    public AcquireTask(final ADIS16448_IMU imu) {
-      this.imu = imu;
-    }
-
-    @Override
-    public void run() {
-      imu.acquire();
-    }
-  }
+  // CRC-16 Look-Up Table
+  private int[] m_adiscrc = {
+    0x0000, 0x17CE, 0x0FDF, 0x1811, 0x1FBE, 0x0870, 0x1061, 0x07AF,
+    0x1F3F, 0x08F1, 0x10E0, 0x072E, 0x0081, 0x174F, 0x0F5E, 0x1890,
+    0x1E3D, 0x09F3, 0x11E2, 0x062C, 0x0183, 0x164D, 0x0E5C, 0x1992,
+    0x0102, 0x16CC, 0x0EDD, 0x1913, 0x1EBC, 0x0972, 0x1163, 0x06AD,
+    0x1C39, 0x0BF7, 0x13E6, 0x0428, 0x0387, 0x1449, 0x0C58, 0x1B96,
+    0x0306, 0x14C8, 0x0CD9, 0x1B17, 0x1CB8, 0x0B76, 0x1367, 0x04A9,
+    0x0204, 0x15CA, 0x0DDB, 0x1A15, 0x1DBA, 0x0A74, 0x1265, 0x05AB,
+    0x1D3B, 0x0AF5, 0x12E4, 0x052A, 0x0285, 0x154B, 0x0D5A, 0x1A94,
+    0x1831, 0x0FFF, 0x17EE, 0x0020, 0x078F, 0x1041, 0x0850, 0x1F9E,
+    0x070E, 0x10C0, 0x08D1, 0x1F1F, 0x18B0, 0x0F7E, 0x176F, 0x00A1,
+    0x060C, 0x11C2, 0x09D3, 0x1E1D, 0x19B2, 0x0E7C, 0x166D, 0x01A3,
+    0x1933, 0x0EFD, 0x16EC, 0x0122, 0x068D, 0x1143, 0x0952, 0x1E9C,
+    0x0408, 0x13C6, 0x0BD7, 0x1C19, 0x1BB6, 0x0C78, 0x1469, 0x03A7,
+    0x1B37, 0x0CF9, 0x14E8, 0x0326, 0x0489, 0x1347, 0x0B56, 0x1C98,
+    0x1A35, 0x0DFB, 0x15EA, 0x0224, 0x058B, 0x1245, 0x0A54, 0x1D9A,
+    0x050A, 0x12C4, 0x0AD5, 0x1D1B, 0x1AB4, 0x0D7A, 0x156B, 0x02A5,
+    0x1021, 0x07EF, 0x1FFE, 0x0830, 0x0F9F, 0x1851, 0x0040, 0x178E,
+    0x0F1E, 0x18D0, 0x00C1, 0x170F, 0x10A0, 0x076E, 0x1F7F, 0x08B1,
+    0x0E1C, 0x19D2, 0x01C3, 0x160D, 0x11A2, 0x066C, 0x1E7D, 0x09B3,
+    0x1123, 0x06ED, 0x1EFC, 0x0932, 0x0E9D, 0x1953, 0x0142, 0x168C,
+    0x0C18, 0x1BD6, 0x03C7, 0x1409, 0x13A6, 0x0468, 0x1C79, 0x0BB7,
+    0x1327, 0x04E9, 0x1CF8, 0x0B36, 0x0C99, 0x1B57, 0x0346, 0x1488,
+    0x1225, 0x05EB, 0x1DFA, 0x0A34, 0x0D9B, 0x1A55, 0x0244, 0x158A,
+    0x0D1A, 0x1AD4, 0x02C5, 0x150B, 0x12A4, 0x056A, 0x1D7B, 0x0AB5,
+    0x0810, 0x1FDE, 0x07CF, 0x1001, 0x17AE, 0x0060, 0x1871, 0x0FBF,
+    0x172F, 0x00E1, 0x18F0, 0x0F3E, 0x0891, 0x1F5F, 0x074E, 0x1080,
+    0x162D, 0x01E3, 0x19F2, 0x0E3C, 0x0993, 0x1E5D, 0x064C, 0x1182,
+    0x0912, 0x1EDC, 0x06CD, 0x1103, 0x16AC, 0x0162, 0x1973, 0x0EBD,
+    0x1429, 0x03E7, 0x1BF6, 0x0C38, 0x0B97, 0x1C59, 0x0448, 0x1386,
+    0x0B16, 0x1CD8, 0x04C9, 0x1307, 0x14A8, 0x0366, 0x1B77, 0x0CB9,
+    0x0A14, 0x1DDA, 0x05CB, 0x1205, 0x15AA, 0x0264, 0x1A75, 0x0DBB,
+    0x152B, 0x02E5, 0x1AF4, 0x0D3A, 0x0A95, 0x1D5B, 0x054A, 0x1284
+  };
 
   /** Creates a new ADIS16448_IMU object. */
   public ADIS16448_IMU() {
@@ -283,7 +243,7 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
     m_yaw_axis = yaw_axis;
     m_spi_port = port;
 
-    m_acquire_task = new Thread(new AcquireTask(this));
+    m_acquire_task = new Thread(this::acquire);
 
     m_simDevice = SimDevice.create("Gyro:ADIS16448", port.value);
     if (m_simDevice != null) {
@@ -301,22 +261,22 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
 
     if (m_simDevice == null) {
       // Force the IMU reset pin to toggle on startup (doesn't require DS enable)
-      m_reset_out = new DigitalOutput(18); // Drive MXP DIO8 low
-      Timer.delay(0.01); // Wait 10ms
-      m_reset_out.close();
+      var reset_out = new DigitalOutput(18); // Drive MXP DIO8 low
+      Timer.delay(0.01);
+      reset_out.close();
       m_reset_in = new DigitalInput(18); // Set MXP DIO8 high
-      Timer.delay(0.25); // Wait 250ms
+      Timer.delay(0.25); // Wait for reset to complete
 
       configCalTime(cal_time);
 
       if (!switchToStandardSPI()) {
         return;
       }
-
+      boolean needsFlash = false;
       // Set IMU internal decimation to 1 (ODR = 819.2 SPS / (1 + 1) = 409.6Hz), BW = 204.8Hz
       if (readRegister(SMPL_PRD) != 0x0001) {
         writeRegister(SMPL_PRD, 0x0001);
-        m_needs_flash = true;
+        needsFlash = true;
         DriverStation.reportWarning(
             "ADIS16448: SMPL_PRD register configuration inconsistent! Scheduling flash update.",
             false);
@@ -325,7 +285,7 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
       // Set data ready polarity (LOW = Good Data) on DIO1 (PWM0 on MXP)
       if (readRegister(MSC_CTRL) != 0x0016) {
         writeRegister(MSC_CTRL, 0x0016);
-        m_needs_flash = true;
+        needsFlash = true;
         DriverStation.reportWarning(
             "ADIS16448: MSC_CTRL register configuration inconsistent! Scheduling flash update.",
             false);
@@ -334,7 +294,7 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
       // Disable IMU internal Bartlett filter (204Hz BW is sufficient) and set IMU scale factor
       if (readRegister(SENS_AVG) != 0x0400) {
         writeRegister(SENS_AVG, 0x0400);
-        m_needs_flash = true;
+        needsFlash = true;
         DriverStation.reportWarning(
             "ADIS16448: SENS_AVG register configuration inconsistent! Scheduling flash update.",
             false);
@@ -342,7 +302,7 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
       // Clear offset registers
       if (readRegister(XGYRO_OFF) != 0x0000) {
         writeRegister(XGYRO_OFF, 0x0000);
-        m_needs_flash = true;
+        needsFlash = true;
         DriverStation.reportWarning(
             "ADIS16448: XGYRO_OFF register configuration inconsistent! Scheduling flash update.",
             false);
@@ -350,7 +310,7 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
 
       if (readRegister(YGYRO_OFF) != 0x0000) {
         writeRegister(YGYRO_OFF, 0x0000);
-        m_needs_flash = true;
+        needsFlash = true;
         DriverStation.reportWarning(
             "ADIS16448: YGYRO_OFF register configuration inconsistent! Scheduling flash update.",
             false);
@@ -358,24 +318,23 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
 
       if (readRegister(ZGYRO_OFF) != 0x0000) {
         writeRegister(ZGYRO_OFF, 0x0000);
-        m_needs_flash = true;
+        needsFlash = true;
         DriverStation.reportWarning(
             "ADIS16448: ZGYRO_OFF register configuration inconsistent! Scheduling flash update.",
             false);
       }
 
       // If any registers on the IMU don't match the config, trigger a flash update
-      if (m_needs_flash) {
+      if (needsFlash) {
         DriverStation.reportWarning(
             "ADIS16448: Register configuration changed! Starting IMU flash update.", false);
         writeRegister(GLOB_CMD, 0x0008);
         // Wait long enough for the flash update to finish (75ms minimum as per the datasheet)
         Timer.delay(0.5);
         DriverStation.reportWarning("ADIS16448: Flash update finished!", false);
-        m_needs_flash = false;
       } else {
         DriverStation.reportWarning(
-            "ADIS16448: and RAM configuration consistent. No flash update required!", false);
+            "ADIS16448: Flash and RAM configuration consistent. No flash update required!", false);
       }
 
       // Configure standard SPI
@@ -389,13 +348,11 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
         Thread.sleep((long) (m_calibration_time.value * 1.2 * 1000));
       } catch (InterruptedException e) {
       }
-      // Execute calibration routine
+      // Execute calibration routine and reset accumulated offsets
       calibrate();
-      // Reset accumulated offsets
-      reset();
       // Indicate to the acquire loop that we're done starting up
       m_start_up_mode = false;
-      // Let the user know the IMU was initiallized successfully
+      // Let the user know the IMU was initialized successfully
       DriverStation.reportWarning("ADIS16448 IMU Successfully Initialized!", false);
       // Drive MXP PWM5 (IMU ready LED) low (active low)
       m_status_led = new DigitalOutput(19);
@@ -418,34 +375,14 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
     return m_connected;
   }
 
-  private static int toUShort(byte[] buf) {
-    return (((buf[0] & 0xFF) << 8) + ((buf[1] & 0xFF) << 0));
+  private static int toUShort(int upper, int lower) {
+    return ((upper & 0xFF) << 8) + (lower & 0xFF);
   }
 
-  private static int toUByte(int... buf) {
-    return (buf[0] & 0xFF);
+  private static int toShort(int upper, int lower) {
+    return (short) (((upper & 0xFF) << 8) + (lower & 0xFF));
   }
 
-  private static int toUShort(int... buf) {
-    return (((buf[0] & 0xFF) << 8) + (buf[1] & 0xFF));
-  }
-
-  /** */
-  private static long toULong(int sint) {
-    return sint & 0x00000000FFFFFFFFL;
-  }
-
-  /** */
-  private static int toShort(int... buf) {
-    return (short) (((buf[0] & 0xFF) << 8) + (buf[1] & 0xFF));
-  }
-
-  /** */
-  private static int toInt(int... buf) {
-    return (buf[0] & 0xFF) << 24 | (buf[1] & 0xFF) << 16 | (buf[2] & 0xFF) << 8 | (buf[3] & 0xFF);
-  }
-
-  /** */
   private boolean switchToStandardSPI() {
     // Check to see whether the acquire thread is active. If so, wait for it to stop
     // producing data.
@@ -464,8 +401,7 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
         // We need to get rid of all the garbage left in the auto SPI buffer after
         // stopping it.
         // Sometimes data magically reappears, so we have to check the buffer size a
-        // couple of times
-        // to be sure we got it all. Yuck.
+        // couple of times to be sure we got it all. Yuck.
         int[] trashBuffer = new int[200];
         try {
           Thread.sleep(100);
@@ -473,9 +409,9 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
         }
         int data_count = m_spi.readAutoReceivedData(trashBuffer, 0, 0);
         while (data_count > 0) {
-          /* Dequeue 200 at a time, or the remainder of the buffer if less than 200 */
+          // Dequeue 200 at a time, or the remainder of the buffer if less than 200
           m_spi.readAutoReceivedData(trashBuffer, Math.min(200, data_count), 0);
-          /* Update remaining buffer count */
+          // Update remaining buffer count
           data_count = m_spi.readAutoReceivedData(trashBuffer, 0, 0);
         }
         System.out.println("Paused auto SPI successfully.");
@@ -511,14 +447,11 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
     }
   }
 
-  /** */
   boolean switchToAutoSPI() {
     // No SPI port has been set up. Go set one up first.
-    if (m_spi == null) {
-      if (!switchToStandardSPI()) {
-        DriverStation.reportError("Failed to start/restart auto SPI", false);
-        return false;
-      }
+    if (m_spi == null && !switchToStandardSPI()) {
+      DriverStation.reportError("Failed to start/restart auto SPI", false);
+      return false;
     }
     // Only set up the interrupt if needed.
     if (m_auto_interrupt == null) {
@@ -537,7 +470,6 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
     // Kick off auto SPI (Note: Device configuration impossible after auto SPI is
     // activated)
     m_spi.startAutoTrigger(m_auto_interrupt, true, false);
-
     // Check to see if the acquire thread is running. If not, kick one off.
     if (!m_acquire_task.isAlive()) {
       m_first_run = true;
@@ -566,19 +498,14 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
    * @return 0 if success, 1 if no change, 2 if error.
    */
   public int configDecRate(int decimationRate) {
-    // Switches the active SPI port to standard SPI mode, writes a new value to
-    // the DECIMATE register in the IMU, and re-enables auto SPI.
-    //
-    // This function enters standard SPI mode, writes a new DECIMATE setting to
-    // the IMU, adjusts the sample scale factor, and re-enters auto SPI mode.
-    int writeValue;
-    int readbackValue;
+    // Switches the active SPI port to standard SPI mode, writes a new value to the DECIMATE
+    // register in the IMU, adjusts the sample scale factor, and re-enables auto SPI.
     if (!switchToStandardSPI()) {
       DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false);
       return 2;
     }
 
-    /* Check max */
+    // Check max
     if (decimationRate > 9) {
       DriverStation.reportError(
           "Attempted to write an invalid decimation value. Capping at 9", false);
@@ -590,16 +517,16 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
       decimationRate = 0;
     }
 
-    /* Shift decimation setting to correct position and select internal sync */
-    writeValue = (decimationRate << 8) | 0x1;
+    // Shift decimation setting to correct position and select internal sync
+    int writeValue = (decimationRate << 8) | 0x1;
 
-    /* Apply to IMU */
+    // Apply to IMU
     writeRegister(SMPL_PRD, writeValue);
 
-    /* Perform read back to verify write */
-    readbackValue = readRegister(SMPL_PRD);
+    // Perform read back to verify write
+    int readbackValue = readRegister(SMPL_PRD);
 
-    /* Throw error for invalid write */
+    // Throw error for invalid write
     if (readbackValue != writeValue) {
       DriverStation.reportError("ADIS16448 SMPL_PRD write failed.", false);
     }
@@ -612,7 +539,7 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
   }
 
   /**
-   * Configures calibration time
+   * Configures calibration time.
    *
    * @param new_cal_time New calibration time
    * @return 1 if the new calibration time is the same as the current one else 0
@@ -664,17 +591,12 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
       m_gyro_rate_offset_x = accum_gyro_rate_x / gyroAverageSize;
       m_gyro_rate_offset_y = accum_gyro_rate_y / gyroAverageSize;
       m_gyro_rate_offset_z = accum_gyro_rate_z / gyroAverageSize;
-      m_integ_gyro_angle_x = 0.0;
-      m_integ_gyro_angle_y = 0.0;
-      m_integ_gyro_angle_z = 0.0;
-      // System.out.println("Avg Size: " + gyroAverageSize + "X Off: " +
-      // m_gyro_rate_offset_x + "Y Off: " + m_gyro_rate_offset_y + "Z Off: " +
-      // m_gyro_rate_offset_z);
+      reset();
     }
   }
 
   /**
-   * Sets the yaw axis
+   * Sets the yaw axis.
    *
    * @param yaw_axis The new yaw axis to use
    * @return 1 if the new yaw axis is the same as the current one else 0.
@@ -689,16 +611,12 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
   }
 
   private int readRegister(final int reg) {
-    // ByteBuffer buf = ByteBuffer.allocateDirect(2);
-    final byte[] buf = new byte[2];
-    // buf.order(ByteOrder.BIG_ENDIAN);
-    buf[0] = (byte) (reg & 0x7f);
-    buf[1] = (byte) 0;
+    final byte[] buf = {(byte) (reg & 0x7f), 0};
 
     m_spi.write(buf, 2);
     m_spi.read(false, buf, 2);
 
-    return toUShort(buf);
+    return toUShort(buf[0], buf[1]);
   }
 
   private void writeRegister(final int reg, final int val) {
@@ -759,7 +677,6 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
     System.out.println("Finished cleaning up after the IMU driver.");
   }
 
-  /** */
   private void acquire() {
     // Set data packet length
     final int dataset_len = 29; // 18 data points + timestamp
@@ -767,36 +684,11 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
 
     // Set up buffers and variables
     int[] buffer = new int[BUFFER_SIZE];
-    int data_count = 0;
-    int data_remainder = 0;
-    int data_to_read = 0;
-    int bufferAvgIndex = 0;
     double previous_timestamp = 0.0;
-    double delta_angle = 0.0;
-    double gyro_rate_x = 0.0;
-    double gyro_rate_y = 0.0;
-    double gyro_rate_z = 0.0;
-    double accel_x = 0.0;
-    double accel_y = 0.0;
-    double accel_z = 0.0;
-    double mag_x = 0.0;
-    double mag_y = 0.0;
-    double mag_z = 0.0;
-    double baro = 0.0;
-    double temp = 0.0;
-    double gyro_rate_x_si = 0.0;
-    double gyro_rate_y_si = 0.0;
-    double gyro_rate_z_si = 0.0;
-    double accel_x_si = 0.0;
-    double accel_y_si = 0.0;
-    double accel_z_si = 0.0;
     double compAngleX = 0.0;
     double compAngleY = 0.0;
-    double accelAngleX = 0.0;
-    double accelAngleY = 0.0;
-
     while (true) {
-      // Sleep loop for 5ms
+      // Wait for data
       try {
         Thread.sleep(5);
       } catch (InterruptedException e) {
@@ -805,91 +697,81 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
       if (m_thread_active) {
         m_thread_idle = false;
 
-        data_count =
-            m_spi.readAutoReceivedData(
-                buffer, 0, 0); // Read number of bytes currently stored in the buffer
-        data_remainder =
-            data_count % dataset_len; // Check if frame is incomplete. Add 1 because of timestamp
-        data_to_read = data_count - data_remainder; // Remove incomplete data from read count
+        // Read number of bytes currently stored in the buffer
+        int data_count = m_spi.readAutoReceivedData(buffer, 0, 0);
+        // Check if frame is incomplete. Add 1 because of timestamp
+        int data_remainder = data_count % dataset_len;
+        // Remove incomplete data from read count
+        int data_to_read = data_count - data_remainder;
         if (data_to_read > BUFFER_SIZE) {
           DriverStation.reportWarning(
               "ADIS16448 data processing thread overrun has occurred!", false);
           data_to_read = BUFFER_SIZE - (BUFFER_SIZE % dataset_len);
         }
-        m_spi.readAutoReceivedData(
-            buffer, data_to_read, 0); // Read data from DMA buffer (only complete sets)
+        // Read data from DMA buffer (only complete sets)
+        m_spi.readAutoReceivedData(buffer, data_to_read, 0);
 
         // Could be multiple data sets in the buffer. Handle each one.
         for (int i = 0; i < data_to_read; i += dataset_len) {
           // Calculate CRC-16 on each data packet
           int calc_crc = 0x0000FFFF; // Starting word
-          int read_byte = 0;
-          int imu_crc = 0;
-          for (int k = 5;
-              k < 27;
-              k += 2) { // Cycle through XYZ GYRO, XYZ ACCEL, XYZ MAG, BARO, TEMP (Ignore Status &
-            // CRC)
-            read_byte = buffer[i + k + 1]; // Process LSB
-            calc_crc = (calc_crc >>> 8) ^ adiscrc[(calc_crc & 0x000000FF) ^ read_byte];
-            read_byte = buffer[i + k]; // Process MSB
-            calc_crc = (calc_crc >>> 8) ^ adiscrc[(calc_crc & 0x000000FF) ^ read_byte];
+          // Cycle through XYZ GYRO, XYZ ACCEL, XYZ MAG, BARO, TEMP (Ignore Status & CRC)
+          for (int k = 5; k < 27; k += 2) {
+            // Process LSB
+            calc_crc = (calc_crc >>> 8) ^ m_adiscrc[(calc_crc & 0x000000FF) ^ buffer[i + k + 1]];
+            // Process MSB
+            calc_crc = (calc_crc >>> 8) ^ m_adiscrc[(calc_crc & 0x000000FF) ^ buffer[i + k]];
           }
-          calc_crc = ~calc_crc & 0xFFFF; // Complement
-          calc_crc = ((calc_crc << 8) | (calc_crc >> 8)) & 0xFFFF; // Flip LSB & MSB
-          imu_crc = toUShort(buffer[i + 27], buffer[i + 28]); // Extract DUT CRC from data buffer
+          // Complement
+          calc_crc = ~calc_crc & 0xFFFF;
+          // Flip LSB & MSB
+          calc_crc = ((calc_crc << 8) | (calc_crc >> 8)) & 0xFFFF;
+          // Extract DUT CRC from data buffer
+          int imu_crc = toUShort(buffer[i + 27], buffer[i + 28]);
 
           if (calc_crc == imu_crc) {
             // Timestamp is at buffer[i]
             m_dt = (buffer[i] - previous_timestamp) / 1000000.0;
 
             // Scale sensor data
-            gyro_rate_x = (toShort(buffer[i + 5], buffer[i + 6]) * 0.04);
-            gyro_rate_y = (toShort(buffer[i + 7], buffer[i + 8]) * 0.04);
-            gyro_rate_z = (toShort(buffer[i + 9], buffer[i + 10]) * 0.04);
-            accel_x = (toShort(buffer[i + 11], buffer[i + 12]) * 0.833);
-            accel_y = (toShort(buffer[i + 13], buffer[i + 14]) * 0.833);
-            accel_z = (toShort(buffer[i + 15], buffer[i + 16]) * 0.833);
-            mag_x = (toShort(buffer[i + 17], buffer[i + 18]) * 0.1429);
-            mag_y = (toShort(buffer[i + 19], buffer[i + 20]) * 0.1429);
-            mag_z = (toShort(buffer[i + 21], buffer[i + 22]) * 0.1429);
-            baro = (toShort(buffer[i + 23], buffer[i + 24]) * 0.02);
-            temp = (toShort(buffer[i + 25], buffer[i + 26]) * 0.07386 + 31.0);
+            double gyro_rate_x = toShort(buffer[i + 5], buffer[i + 6]) * 0.04;
+            double gyro_rate_y = toShort(buffer[i + 7], buffer[i + 8]) * 0.04;
+            double gyro_rate_z = toShort(buffer[i + 9], buffer[i + 10]) * 0.04;
+            double accel_x = toShort(buffer[i + 11], buffer[i + 12]) * 0.833;
+            double accel_y = toShort(buffer[i + 13], buffer[i + 14]) * 0.833;
+            double accel_z = toShort(buffer[i + 15], buffer[i + 16]) * 0.833;
+            double mag_x = toShort(buffer[i + 17], buffer[i + 18]) * 0.1429;
+            double mag_y = toShort(buffer[i + 19], buffer[i + 20]) * 0.1429;
+            double mag_z = toShort(buffer[i + 21], buffer[i + 22]) * 0.1429;
+            double baro = toShort(buffer[i + 23], buffer[i + 24]) * 0.02;
+            double temp = toShort(buffer[i + 25], buffer[i + 26]) * 0.07386 + 31.0;
 
             // Convert scaled sensor data to SI units (for tilt calculations)
             // TODO: Should the unit outputs be selectable?
-            gyro_rate_x_si = gyro_rate_x * deg_to_rad;
-            gyro_rate_y_si = gyro_rate_y * deg_to_rad;
-            gyro_rate_z_si = gyro_rate_z * deg_to_rad;
-            accel_x_si = accel_x * grav;
-            accel_y_si = accel_y * grav;
-            accel_z_si = accel_z * grav;
+            double gyro_rate_x_si = Math.toRadians(gyro_rate_x);
+            double gyro_rate_y_si = Math.toRadians(gyro_rate_y);
+            // double gyro_rate_z_si = Math.toRadians(gyro_rate_z);
+            double accel_x_si = accel_x * grav;
+            double accel_y_si = accel_y * grav;
+            double accel_z_si = accel_z * grav;
             // Store timestamp for next iteration
             previous_timestamp = buffer[i];
             // Calculate alpha for use with the complementary filter
-            m_alpha = m_tau / (m_tau + m_dt);
+            m_alpha = 1.0 / (1.0 + m_dt);
+            // Run inclinometer calculations
+            double accelAngleX =
+                Math.atan2(
+                    -accel_x_si,
+                    Math.sqrt((accel_y_si * accel_y_si) + (-accel_z_si * -accel_z_si)));
+            double accelAngleY =
+                Math.atan2(
+                    accel_y_si,
+                    Math.sqrt((-accel_x_si * -accel_x_si) + (-accel_z_si * -accel_z_si)));
             // Calculate complementary filter
             if (m_first_run) {
-              // Set up inclinometer calculations for first run
-              accelAngleX =
-                  Math.atan2(
-                      -accel_x_si,
-                      Math.sqrt((accel_y_si * accel_y_si) + (-accel_z_si * -accel_z_si)));
-              accelAngleY =
-                  Math.atan2(
-                      accel_y_si,
-                      Math.sqrt((-accel_x_si * -accel_x_si) + (-accel_z_si * -accel_z_si)));
               compAngleX = accelAngleX;
               compAngleY = accelAngleY;
             } else {
-              // Run inclinometer calculations
-              accelAngleX =
-                  Math.atan2(
-                      -accel_x_si,
-                      Math.sqrt((accel_y_si * accel_y_si) + (-accel_z_si * -accel_z_si)));
-              accelAngleY =
-                  Math.atan2(
-                      accel_y_si,
-                      Math.sqrt((-accel_x_si * -accel_x_si) + (-accel_z_si * -accel_z_si)));
               accelAngleX = formatAccelRange(accelAngleX, -accel_z_si);
               accelAngleY = formatAccelRange(accelAngleY, -accel_z_si);
               compAngleX = compFilterProcess(compAngleX, accelAngleX, -gyro_rate_y_si);
@@ -900,13 +782,11 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
             synchronized (this) {
               // Ignore first, integrated sample
               if (m_first_run) {
-                m_integ_gyro_angle_x = 0.0;
-                m_integ_gyro_angle_y = 0.0;
-                m_integ_gyro_angle_z = 0.0;
+                reset();
               } else {
                 // Accumulate gyro for offset calibration
                 // Add to buffer
-                bufferAvgIndex = m_accum_count % m_avg_size;
+                int bufferAvgIndex = m_accum_count % m_avg_size;
                 m_offset_data_gyro_rate_x[bufferAvgIndex] = gyro_rate_x;
                 m_offset_data_gyro_rate_y[bufferAvgIndex] = gyro_rate_y;
                 m_offset_data_gyro_rate_z[bufferAvgIndex] = gyro_rate_z;
@@ -925,10 +805,10 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
                 m_mag_z = mag_z;
                 m_baro = baro;
                 m_temp = temp;
-                m_compAngleX = compAngleX * rad_to_deg;
-                m_compAngleY = compAngleY * rad_to_deg;
-                m_accelAngleX = accelAngleX * rad_to_deg;
-                m_accelAngleY = accelAngleY * rad_to_deg;
+                m_compAngleX = Math.toDegrees(compAngleX);
+                m_compAngleY = Math.toDegrees(compAngleY);
+                m_accelAngleX = Math.toDegrees(accelAngleX);
+                m_accelAngleY = Math.toDegrees(accelAngleY);
                 // Accumulate gyro for angle integration and publish to global variables
                 m_integ_gyro_angle_x += (gyro_rate_x - m_gyro_rate_offset_x) * m_dt;
                 m_integ_gyro_angle_y += (gyro_rate_y - m_gyro_rate_offset_y) * m_dt;
@@ -937,67 +817,17 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
               // System.out.println("Good CRC");
             }
             m_first_run = false;
-          } else {
-            // System.out.println("Bad CRC");
-            /*
-             * System.out.println("Calc CRC: " + calc_crc);
-             * System.out.println("IMU CRC: " + imu_crc);
-             * System.out.println(
-             * buffer[i] + " " +
-             * (buffer[i + 1]) + " " + (buffer[i + 2]) + " " +
-             * (buffer[i + 3]) + " " + (buffer[i + 4]) + " " +
-             * (buffer[i + 5]) + " " + (buffer[i + 6]) + " " +
-             * (buffer[i + 7]) + " " + (buffer[i + 8]) + " " +
-             * (buffer[i + 9]) + " " + (buffer[i + 10]) + " " +
-             * (buffer[i + 11]) + " " + (buffer[i + 12]) + " " +
-             * (buffer[i + 13]) + " " + (buffer[i + 14]) + " " +
-             * (buffer[i + 15]) + " " + (buffer[i + 16]) + " " +
-             * (buffer[i + 17]) + " " + (buffer[i + 18]) + " " +
-             * (buffer[i + 19]) + " " + (buffer[i + 20]) + " " +
-             * (buffer[i + 21]) + " " + (buffer[i + 22]) + " " +
-             * (buffer[i + 23]) + " " + (buffer[i + 24]) + " " +
-             * (buffer[i + 25]) + " " + (buffer[i + 26]) + " " +
-             * (buffer[i + 27]) + " " + (buffer[i + 28]));
-             */
           }
         }
       } else {
         m_thread_idle = true;
-        data_count = 0;
-        data_remainder = 0;
-        data_to_read = 0;
         previous_timestamp = 0.0;
-        delta_angle = 0.0;
-        gyro_rate_x = 0.0;
-        gyro_rate_y = 0.0;
-        gyro_rate_z = 0.0;
-        accel_x = 0.0;
-        accel_y = 0.0;
-        accel_z = 0.0;
-        mag_x = 0.0;
-        mag_y = 0.0;
-        mag_z = 0.0;
-        baro = 0.0;
-        temp = 0.0;
-        gyro_rate_x_si = 0.0;
-        gyro_rate_y_si = 0.0;
-        gyro_rate_z_si = 0.0;
-        accel_x_si = 0.0;
-        accel_y_si = 0.0;
-        accel_z_si = 0.0;
         compAngleX = 0.0;
         compAngleY = 0.0;
-        accelAngleX = 0.0;
-        accelAngleY = 0.0;
       }
     }
   }
 
-  /**
-   * @param compAngle
-   * @param accAngle
-   * @return
-   */
   private double formatFastConverge(double compAngle, double accAngle) {
     if (compAngle > accAngle + Math.PI) {
       compAngle = compAngle - 2.0 * Math.PI;
@@ -1007,25 +837,6 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
     return compAngle;
   }
 
-  /**
-   * @param compAngle
-   * @return
-   */
-  private double formatRange0to2PI(double compAngle) {
-    while (compAngle >= 2 * Math.PI) {
-      compAngle = compAngle - 2.0 * Math.PI;
-    }
-    while (compAngle < 0.0) {
-      compAngle = compAngle + 2.0 * Math.PI;
-    }
-    return compAngle;
-  }
-
-  /**
-   * @param accelAngle
-   * @param accelZ
-   * @return
-   */
   private double formatAccelRange(double accelAngle, double accelZ) {
     if (accelZ < 0.0) {
       accelAngle = Math.PI - accelAngle;
@@ -1035,20 +846,10 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
     return accelAngle;
   }
 
-  /**
-   * @param compAngle
-   * @param accelAngle
-   * @param omega
-   * @return
-   */
   private double compFilterProcess(double compAngle, double accelAngle, double omega) {
     compAngle = formatFastConverge(compAngle, accelAngle);
     compAngle = m_alpha * (compAngle + omega * m_dt) + (1.0 - m_alpha) * accelAngle;
-    compAngle = formatRange0to2PI(compAngle);
-    if (compAngle > Math.PI) {
-      compAngle = compAngle - 2.0 * Math.PI;
-    }
-    return compAngle;
+    return MathUtil.angleModulus(compAngle);
   }
 
   /**
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java
index d320167c4f..337c0f2370 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java
@@ -2,55 +2,25 @@
 // 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.
 
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2016. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
 package edu.wpi.first.wpilibj;
 
-// import java.lang.FdLibm.Pow;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.hal.SimBoolean;
 import edu.wpi.first.hal.SimDevice;
 import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.util.sendable.Sendable;
 import edu.wpi.first.util.sendable.SendableBuilder;
-import java.nio.ByteBuffer;
-import java.nio.ByteOrder;
 
 // CHECKSTYLE.OFF: TypeName
 // CHECKSTYLE.OFF: MemberName
-// CHECKSTYLE.OFF: SummaryJavadoc
-// CHECKSTYLE.OFF: UnnecessaryParentheses
-// CHECKSTYLE.OFF: OverloadMethodsDeclarationOrder
-// CHECKSTYLE.OFF: NonEmptyAtclauseDescription
-// CHECKSTYLE.OFF: MissingOverride
-// CHECKSTYLE.OFF: AtclauseOrder
 // CHECKSTYLE.OFF: LocalVariableName
-// CHECKSTYLE.OFF: RedundantModifier
-// CHECKSTYLE.OFF: AbbreviationAsWordInName
 // CHECKSTYLE.OFF: ParameterName
 // CHECKSTYLE.OFF: EmptyCatchBlock
-// CHECKSTYLE.OFF: MissingJavadocMethod
-// CHECKSTYLE.OFF: MissingSwitchDefault
-// CHECKSTYLE.OFF: VariableDeclarationUsageDistance
-// CHECKSTYLE.OFF: ArrayTypeStyle
 
 /** This class is for the ADIS16470 IMU that connects to the RoboRIO SPI port. */
-@SuppressWarnings({
-  "unused",
-  "PMD.RedundantFieldInitializer",
-  "PMD.ImmutableField",
-  "PMD.SingularField",
-  "PMD.CollapsibleIfStatements",
-  "PMD.MissingOverride",
-  "PMD.EmptyIfStmt",
-  "PMD.EmptyStatementNotInLoop"
-})
+@SuppressWarnings("PMD.RedundantFieldInitializer")
 public class ADIS16470_IMU implements AutoCloseable, Sendable {
   /* ADIS16470 Register Map Declaration */
   private static final int FLASH_CNT = 0x00; // Flash memory write count
@@ -157,29 +127,29 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
 
   /** ADIS16470 calibration times. */
   public enum CalibrationTime {
-    /** 32 ms calibration time */
+    /** 32 ms calibration time. */
     _32ms(0),
-    /** 64 ms calibration time */
+    /** 64 ms calibration time. */
     _64ms(1),
-    /** 128 ms calibration time */
+    /** 128 ms calibration time. */
     _128ms(2),
-    /** 256 ms calibration time */
+    /** 256 ms calibration time. */
     _256ms(3),
-    /** 512 ms calibration time */
+    /** 512 ms calibration time. */
     _512ms(4),
-    /** 1 s calibration time */
+    /** 1 s calibration time. */
     _1s(5),
-    /** 2 s calibration time */
+    /** 2 s calibration time. */
     _2s(6),
-    /** 4 s calibration time */
+    /** 4 s calibration time. */
     _4s(7),
-    /** 8 s calibration time */
+    /** 8 s calibration time. */
     _8s(8),
-    /** 16 s calibration time */
+    /** 16 s calibration time. */
     _16s(9),
-    /** 32 s calibration time */
+    /** 32 s calibration time. */
     _32s(10),
-    /** 64 s calibration time */
+    /** 64 s calibration time. */
     _64s(11);
 
     private final int value;
@@ -196,24 +166,22 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
    * configured by the user to refer to an X, Y, or Z axis.
    */
   public enum IMUAxis {
-    /** The IMU's X axis */
+    /** The IMU's X axis. */
     kX,
-    /** The IMU's Y axis */
+    /** The IMU's Y axis. */
     kY,
-    /** The IMU's Z axis */
+    /** The IMU's Z axis. */
     kZ,
-    /** The user-configured yaw axis */
+    /** The user-configured yaw axis. */
     kYaw,
-    /** The user-configured pitch axis */
+    /** The user-configured pitch axis. */
     kPitch,
-    /** The user-configured roll axis */
+    /** The user-configured roll axis. */
     kRoll,
   }
 
   // Static Constants
-  private static final double delta_angle_sf = 2160.0 / 2147483648.0; /* 2160 / (2^31) */
-  private static final double rad_to_deg = 57.2957795;
-  private static final double deg_to_rad = 0.0174532;
+  private static final double delta_angle_sf = 2160.0 / 2147483648.0; // 2160 / (2^31)
   private static final double grav = 9.81;
 
   // User-specified axes
@@ -225,9 +193,6 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
   private double m_gyro_rate_x = 0.0;
   private double m_gyro_rate_y = 0.0;
   private double m_gyro_rate_z = 0.0;
-  private double m_average_gyro_rate_x = 0.0;
-  private double m_average_gyro_rate_y = 0.0;
-  private double m_average_gyro_rate_z = 0.0;
   private double m_accel_x = 0.0;
   private double m_accel_y = 0.0;
   private double m_accel_z = 0.0;
@@ -240,7 +205,6 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
   // Complementary filter variables
   private double m_dt = 0.0;
   private double m_alpha = 0.0;
-  private double m_tau = 1.0;
   private double m_compAngleX = 0.0;
   private double m_compAngleY = 0.0;
   private double m_accelAngleX = 0.0;
@@ -253,13 +217,11 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
   private volatile boolean m_thread_idle = false;
   private boolean m_auto_configured = false;
   private double m_scaled_sample_rate = 2500.0;
-  private boolean m_needs_flash = false;
 
   // Resources
   private SPI m_spi;
   private SPI.Port m_spi_port;
   private DigitalInput m_auto_interrupt;
-  private DigitalOutput m_reset_out;
   private DigitalInput m_reset_in;
   private DigitalOutput m_status_led;
   private Thread m_acquire_task;
@@ -277,19 +239,6 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
   private SimDouble m_simAccelY;
   private SimDouble m_simAccelZ;
 
-  private static class AcquireTask implements Runnable {
-    private ADIS16470_IMU imu;
-
-    public AcquireTask(ADIS16470_IMU imu) {
-      this.imu = imu;
-    }
-
-    @Override
-    public void run() {
-      imu.acquire();
-    }
-  }
-
   /**
    * Creates a new ADIS16740 IMU object.
    *
@@ -361,7 +310,7 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
     m_calibration_time = cal_time.value;
     m_spi_port = port;
 
-    m_acquire_task = new Thread(new AcquireTask(this));
+    m_acquire_task = new Thread(this::acquire);
 
     m_simDevice = SimDevice.create("Gyro:ADIS16470", port.value);
     if (m_simDevice != null) {
@@ -382,20 +331,20 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
       // Relies on the RIO hardware by default configuring an output as low
       // and configuring an input as high Z. The 10k pull-up resistor internal to the
       // IMU then forces the reset line high for normal operation.
-      m_reset_out = new DigitalOutput(27); // Drive SPI CS2 (IMU RST) low
-      Timer.delay(0.01); // Wait 10ms
-      m_reset_out.close();
+      var reset_out = new DigitalOutput(27); // Drive SPI CS2 (IMU RST) low
+      Timer.delay(0.01);
+      reset_out.close();
       m_reset_in = new DigitalInput(27); // Set SPI CS2 (IMU RST) high
-      Timer.delay(0.25); // Wait 250ms for reset to complete
+      Timer.delay(0.25); // Wait for reset to complete
 
       if (!switchToStandardSPI()) {
         return;
       }
-
+      boolean needsFlash = false;
       // Set IMU internal decimation to 4 (ODR = 2000 SPS / (4 + 1) = 400Hz), BW = 200Hz
       if (readRegister(DEC_RATE) != 0x0004) {
         writeRegister(DEC_RATE, 0x0004);
-        m_needs_flash = true;
+        needsFlash = true;
         DriverStation.reportWarning(
             "ADIS16470: DEC_RATE register configuration inconsistent! Scheduling flash update.",
             false);
@@ -404,7 +353,7 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
       // Set data ready polarity (HIGH = Good Data), Disable gSense Compensation and PoP
       if (readRegister(MSC_CTRL) != 0x0001) {
         writeRegister(MSC_CTRL, 0x0001);
-        m_needs_flash = true;
+        needsFlash = true;
         DriverStation.reportWarning(
             "ADIS16470: MSC_CTRL register configuration inconsistent! Scheduling flash update.",
             false);
@@ -413,28 +362,27 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
       // Disable IMU internal Bartlett filter (200Hz bandwidth is sufficient)
       if (readRegister(FILT_CTRL) != 0x0000) {
         writeRegister(FILT_CTRL, 0x0000);
-        m_needs_flash = true;
+        needsFlash = true;
         DriverStation.reportWarning(
             "ADIS16470: FILT_CTRL register configuration inconsistent! Scheduling flash update.",
             false);
       }
 
       // If any registers on the IMU don't match the config, trigger a flash update
-      if (m_needs_flash) {
+      if (needsFlash) {
         DriverStation.reportWarning(
             "ADIS16470: Register configuration changed! Starting IMU flash update.", false);
         writeRegister(GLOB_CMD, 0x0008);
         // Wait long enough for the flash update to finish (72ms minimum as per the datasheet)
         Timer.delay(0.3);
         DriverStation.reportWarning("ADIS16470: Flash update finished!", false);
-        m_needs_flash = false;
       } else {
         DriverStation.reportWarning(
             "ADIS16470: Flash and RAM configuration consistent. No flash update required!", false);
       }
 
       // Configure continuous bias calibration time based on user setting
-      writeRegister(NULL_CNFG, (m_calibration_time | 0x0700));
+      writeRegister(NULL_CNFG, m_calibration_time | 0x0700);
 
       // Notify DS that IMU calibration delay is active
       DriverStation.reportWarning("ADIS16470: Starting initial calibration delay.", false);
@@ -454,7 +402,7 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
         return;
       }
 
-      // Let the user know the IMU was initiallized successfully
+      // Let the user know the IMU was initialized successfully
       DriverStation.reportWarning("ADIS16470 IMU Successfully Initialized!", false);
 
       // Drive "Ready" LED low
@@ -478,36 +426,12 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
     return m_connected;
   }
 
-  /**
-   * @param buf
-   * @return
-   */
-  private static int toUShort(ByteBuffer buf) {
-    return buf.getShort(0) & 0xFFFF;
+  private static int toShort(int upper, int lower) {
+    return (short) (((upper & 0xFF) << 8) + (lower & 0xFF));
   }
 
-  /**
-   * @param sint
-   * @return
-   */
-  private static long toULong(int sint) {
-    return sint & 0x00000000FFFFFFFFL;
-  }
-
-  /**
-   * @param buf
-   * @return
-   */
-  private static int toShort(int... buf) {
-    return (short) (((buf[0] & 0xFF) << 8) + (buf[1] & 0xFF));
-  }
-
-  /**
-   * @param buf
-   * @return
-   */
-  private static int toInt(int... buf) {
-    return (buf[0] & 0xFF) << 24 | (buf[1] & 0xFF) << 16 | (buf[2] & 0xFF) << 8 | (buf[3] & 0xFF);
+  private static int toInt(int msb, int byte2, int byte3, int lsb) {
+    return (msb & 0xFF) << 24 | (byte2 & 0xFF) << 16 | (byte3 & 0xFF) << 8 | (lsb & 0xFF);
   }
 
   /**
@@ -533,8 +457,7 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
         // We need to get rid of all the garbage left in the auto SPI buffer after
         // stopping it.
         // Sometimes data magically reappears, so we have to check the buffer size a
-        // couple of times
-        // to be sure we got it all. Yuck.
+        // couple of times to be sure we got it all. Yuck.
         int[] trashBuffer = new int[200];
         try {
           Thread.sleep(100);
@@ -585,11 +508,9 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
    */
   boolean switchToAutoSPI() {
     // No SPI port has been set up. Go set one up first.
-    if (m_spi == null) {
-      if (!switchToStandardSPI()) {
-        DriverStation.reportError("Failed to start/restart auto SPI", false);
-        return false;
-      }
+    if (m_spi == null && !switchToStandardSPI()) {
+      DriverStation.reportError("Failed to start/restart auto SPI", false);
+      return false;
     }
     // Only set up the interrupt if needed.
     if (m_auto_interrupt == null) {
@@ -610,7 +531,6 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
     // activated)
     // DR High = Data good (data capture should be triggered on the rising edge)
     m_spi.startAutoTrigger(m_auto_interrupt, true, false);
-
     // Check to see if the acquire thread is running. If not, kick one off.
     if (!m_acquire_task.isAlive()) {
       m_first_run = true;
@@ -633,10 +553,10 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
   }
 
   /**
-   * Configures calibration time
+   * Configures calibration time.
    *
    * @param new_cal_time New calibration time
-   * @return 1 if the new calibration time is the same as the current one else 0
+   * @return 0 if success, 1 if no change, 2 if error.
    */
   public int configCalTime(CalibrationTime new_cal_time) {
     if (m_calibration_time == new_cal_time.value) {
@@ -647,7 +567,7 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
       return 2;
     }
     m_calibration_time = new_cal_time.value;
-    writeRegister(NULL_CNFG, (m_calibration_time | 0x700));
+    writeRegister(NULL_CNFG, m_calibration_time | 0x700);
     if (!switchToAutoSPI()) {
       DriverStation.reportError("Failed to configure/reconfigure auto SPI.", false);
       return 2;
@@ -662,11 +582,8 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
    * @return 0 if success, 1 if no change, 2 if error.
    */
   public int configDecRate(int decimationRate) {
-    // Switches the active SPI port to standard SPI mode, writes a new value to
-    // the DECIMATE register in the IMU, and re-enables auto SPI.
-    //
-    // This function enters standard SPI mode, writes a new DECIMATE setting to
-    // the IMU, adjusts the sample scale factor, and re-enters auto SPI mode.
+    // Switches the active SPI port to standard SPI mode, writes a new value to the DECIMATE
+    // register in the IMU, adjusts the sample scale factor, and re-enables auto SPI.
     if (!switchToStandardSPI()) {
       DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false);
       return 2;
@@ -675,7 +592,7 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
       DriverStation.reportError("Attempted to write an invalid decimation value.", false);
       decimationRate = 1999;
     }
-    m_scaled_sample_rate = (((decimationRate + 1.0) / 2000.0) * 1000000.0);
+    m_scaled_sample_rate = (decimationRate + 1.0) / 2000.0 * 1000000.0;
     writeRegister(DEC_RATE, decimationRate);
     System.out.println("Decimation register: " + readRegister(DEC_RATE));
     if (!switchToAutoSPI()) {
@@ -700,35 +617,22 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
     }
   }
 
-  /**
-   * @param reg
-   * @return
-   */
   private int readRegister(int reg) {
-    ByteBuffer buf = ByteBuffer.allocateDirect(2);
-    buf.order(ByteOrder.BIG_ENDIAN);
-    buf.put(0, (byte) (reg & 0x7f));
-    buf.put(1, (byte) 0);
+    final byte[] buf = {(byte) (reg & 0x7f), 0};
 
     m_spi.write(buf, 2);
     m_spi.read(false, buf, 2);
 
-    return toUShort(buf);
+    return (buf[0] << 8) & buf[1];
   }
 
-  /**
-   * @param reg
-   * @param val
-   */
   private void writeRegister(int reg, int val) {
-    ByteBuffer buf = ByteBuffer.allocateDirect(2);
     // low byte
-    buf.put(0, (byte) (0x80 | reg));
-    buf.put(1, (byte) (val & 0xff));
+    final byte[] buf = {(byte) (0x80 | reg), (byte) (val & 0xff)};
     m_spi.write(buf, 2);
     // high byte
-    buf.put(0, (byte) (0x81 | reg));
-    buf.put(1, (byte) (val >> 8));
+    buf[0] = (byte) (0x81 | reg);
+    buf[1] = (byte) (val >> 8);
     m_spi.write(buf, 2);
   }
 
@@ -764,7 +668,6 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
     System.out.println("Finished cleaning up after the IMU driver.");
   }
 
-  /** */
   private void acquire() {
     // Set data packet length
     final int dataset_len = 27; // 26 data points + timestamp
@@ -772,32 +675,11 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
 
     // Set up buffers and variables
     int[] buffer = new int[BUFFER_SIZE];
-    int data_count = 0;
-    int data_remainder = 0;
-    int data_to_read = 0;
     double previous_timestamp = 0.0;
-    double delta_angle_x = 0.0;
-    double delta_angle_y = 0.0;
-    double delta_angle_z = 0.0;
-    double gyro_rate_x = 0.0;
-    double gyro_rate_y = 0.0;
-    double gyro_rate_z = 0.0;
-    double accel_x = 0.0;
-    double accel_y = 0.0;
-    double accel_z = 0.0;
-    double gyro_rate_x_si = 0.0;
-    double gyro_rate_y_si = 0.0;
-    double gyro_rate_z_si = 0.0;
-    double accel_x_si = 0.0;
-    double accel_y_si = 0.0;
-    double accel_z_si = 0.0;
     double compAngleX = 0.0;
     double compAngleY = 0.0;
-    double accelAngleX = 0.0;
-    double accelAngleY = 0.0;
-
     while (true) {
-      // Sleep loop for 10ms
+      // Wait for data
       try {
         Thread.sleep(10);
       } catch (InterruptedException e) {
@@ -806,108 +688,74 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
       if (m_thread_active) {
         m_thread_idle = false;
 
-        data_count =
-            m_spi.readAutoReceivedData(
-                buffer, 0, 0); // Read number of bytes currently stored in the buffer
-
-        data_remainder =
-            data_count % dataset_len; // Check if frame is incomplete. Add 1 because of timestamp
-        data_to_read = data_count - data_remainder; // Remove incomplete data from read count
-        /* Want to cap the data to read in a single read at the buffer size */
+        // Read number of bytes currently stored in the buffer
+        int data_count = m_spi.readAutoReceivedData(buffer, 0, 0);
+        // Check if frame is incomplete. Add 1 because of timestamp
+        int data_remainder = data_count % dataset_len;
+        // Remove incomplete data from read count
+        int data_to_read = data_count - data_remainder;
+        // Want to cap the data to read in a single read at the buffer size
         if (data_to_read > BUFFER_SIZE) {
           DriverStation.reportWarning(
               "ADIS16470 data processing thread overrun has occurred!", false);
           data_to_read = BUFFER_SIZE - (BUFFER_SIZE % dataset_len);
         }
-        m_spi.readAutoReceivedData(
-            buffer, data_to_read, 0); // Read data from DMA buffer (only complete sets)
+        // Read data from DMA buffer (only complete sets)
+        m_spi.readAutoReceivedData(buffer, data_to_read, 0);
 
         // Could be multiple data sets in the buffer. Handle each one.
         for (int i = 0; i < data_to_read; i += dataset_len) {
           // Timestamp is at buffer[i]
           m_dt = (buffer[i] - previous_timestamp) / 1000000.0;
+          // Get delta angle value for all 3 axes and scale by the elapsed time
+          // (based on timestamp)
+          double elapsed_time = m_scaled_sample_rate / (buffer[i] - previous_timestamp);
+          double delta_angle_x =
+              toInt(buffer[i + 3], buffer[i + 4], buffer[i + 5], buffer[i + 6])
+                  * delta_angle_sf
+                  / elapsed_time;
+          double delta_angle_y =
+              toInt(buffer[i + 7], buffer[i + 8], buffer[i + 9], buffer[i + 10])
+                  * delta_angle_sf
+                  / elapsed_time;
+          double delta_angle_z =
+              toInt(buffer[i + 11], buffer[i + 12], buffer[i + 13], buffer[i + 14])
+                  * delta_angle_sf
+                  / elapsed_time;
 
-          /*
-           * System.out.println(((toInt(buffer[i + 3], buffer[i + 4], buffer[i + 5],
-           * buffer[i + 6]))*delta_angle_sf) / ((10000.0 / (buffer[i] -
-           * previous_timestamp)) / 100.0));
-           * // DEBUG: Plot Sub-Array Data in Terminal
-           * for (int j = 0; j < data_to_read; j++) {
-           * System.out.print(buffer[j]);
-           * System.out.print(" ,");
-           * }
-           * System.out.println(" ");
-           * //System.out.println(((toInt(buffer[i + 3], buffer[i + 4], buffer[i + 5],
-           * buffer[i + 6]))*delta_angle_sf) / ((10000.0 / (buffer[i] -
-           * previous_timestamp)) / 100.0) + "," + buffer[3] + "," + buffer[4] + "," +
-           * buffer[5] + "," + buffer[6]
-           * /*toShort(buffer[7], buffer[8]) + "," +
-           * toShort(buffer[9], buffer[10]) + "," +
-           * toShort(buffer[11], buffer[12]) + "," +
-           * toShort(buffer[13], buffer[14]) + "," +
-           * toShort(buffer[15], buffer[16]) + ","
-           * + toShort(buffer[17], buffer[18]));
-           */
+          double gyro_rate_x = toShort(buffer[i + 15], buffer[i + 16]) / 10.0;
+          double gyro_rate_y = toShort(buffer[i + 17], buffer[i + 18]) / 10.0;
+          double gyro_rate_z = toShort(buffer[i + 19], buffer[i + 20]) / 10.0;
 
-          /*
-           * Get delta angle value for all 3 axes and scale by the elapsed time
-           * (based on timestamp)
-           */
-          delta_angle_x =
-              (toInt(buffer[i + 3], buffer[i + 4], buffer[i + 5], buffer[i + 6]) * delta_angle_sf)
-                  / (m_scaled_sample_rate / (buffer[i] - previous_timestamp));
-          delta_angle_y =
-              (toInt(buffer[i + 7], buffer[i + 8], buffer[i + 9], buffer[i + 10]) * delta_angle_sf)
-                  / (m_scaled_sample_rate / (buffer[i] - previous_timestamp));
-          delta_angle_z =
-              (toInt(buffer[i + 11], buffer[i + 12], buffer[i + 13], buffer[i + 14])
-                      * delta_angle_sf)
-                  / (m_scaled_sample_rate / (buffer[i] - previous_timestamp));
-
-          gyro_rate_x = (toShort(buffer[i + 15], buffer[i + 16]) / 10.0);
-          gyro_rate_y = (toShort(buffer[i + 17], buffer[i + 18]) / 10.0);
-          gyro_rate_z = (toShort(buffer[i + 19], buffer[i + 20]) / 10.0);
-
-          accel_x = (toShort(buffer[i + 21], buffer[i + 22]) / 800.0);
-          accel_y = (toShort(buffer[i + 23], buffer[i + 24]) / 800.0);
-          accel_z = (toShort(buffer[i + 25], buffer[i + 26]) / 800.0);
+          double accel_x = toShort(buffer[i + 21], buffer[i + 22]) / 800.0;
+          double accel_y = toShort(buffer[i + 23], buffer[i + 24]) / 800.0;
+          double accel_z = toShort(buffer[i + 25], buffer[i + 26]) / 800.0;
 
           // Convert scaled sensor data to SI units (for tilt calculations)
           // TODO: Should the unit outputs be selectable?
-          gyro_rate_x_si = gyro_rate_x * deg_to_rad;
-          gyro_rate_y_si = gyro_rate_y * deg_to_rad;
-          gyro_rate_z_si = gyro_rate_z * deg_to_rad;
-          accel_x_si = accel_x * grav;
-          accel_y_si = accel_y * grav;
-          accel_z_si = accel_z * grav;
+          double gyro_rate_x_si = Math.toRadians(gyro_rate_x);
+          double gyro_rate_y_si = Math.toRadians(gyro_rate_y);
+          // double gyro_rate_z_si = Math.toRadians(gyro_rate_z);
+          double accel_x_si = accel_x * grav;
+          double accel_y_si = accel_y * grav;
+          double accel_z_si = accel_z * grav;
 
           // Store timestamp for next iteration
           previous_timestamp = buffer[i];
 
-          m_alpha = m_tau / (m_tau + m_dt);
+          m_alpha = 1.0 / (1.0 + m_dt);
 
+          // Run inclinometer calculations
+          double accelAngleX =
+              Math.atan2(
+                  accel_x_si, Math.sqrt((accel_y_si * accel_y_si) + (accel_z_si * accel_z_si)));
+          double accelAngleY =
+              Math.atan2(
+                  accel_y_si, Math.sqrt((accel_x_si * accel_x_si) + (accel_z_si * accel_z_si)));
           if (m_first_run) {
-            // Set up inclinometer calculations for first run
-            accelAngleX =
-                Math.atan2(
-                    accel_x_si, Math.sqrt((accel_y_si * accel_y_si) + (accel_z_si * accel_z_si)));
-            accelAngleY =
-                Math.atan2(
-                    accel_y_si, Math.sqrt((accel_x_si * accel_x_si) + (accel_z_si * accel_z_si)));
             compAngleX = accelAngleX;
             compAngleY = accelAngleY;
-
-            m_average_gyro_rate_x = 0.0;
-            m_average_gyro_rate_y = 0.0;
-            m_average_gyro_rate_z = 0.0;
           } else {
-            // Run inclinometer calculations
-            accelAngleX =
-                Math.atan2(
-                    accel_x_si, Math.sqrt((accel_y_si * accel_y_si) + (accel_z_si * accel_z_si)));
-            accelAngleY =
-                Math.atan2(
-                    accel_y_si, Math.sqrt((accel_x_si * accel_x_si) + (accel_z_si * accel_z_si)));
             accelAngleX = formatAccelRange(accelAngleX, accel_z_si);
             accelAngleY = formatAccelRange(accelAngleY, accel_z_si);
             compAngleX = compFilterProcess(compAngleX, accelAngleX, -gyro_rate_y_si);
@@ -915,15 +763,11 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
           }
 
           synchronized (this) {
-            /* Push data to global variables */
+            // Push data to global variables
             if (m_first_run) {
-              /*
-               * Don't accumulate first run. previous_timestamp will be "very" old and the
-               * integration will end up way off
-               */
-              m_integ_angle_x = 0.0;
-              m_integ_angle_y = 0.0;
-              m_integ_angle_z = 0.0;
+              // Don't accumulate first run. previous_timestamp will be "very" old and the
+              // integration will end up way off
+              reset();
             } else {
               m_integ_angle_x += delta_angle_x;
               m_integ_angle_y += delta_angle_y;
@@ -935,57 +779,22 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
             m_accel_x = accel_x;
             m_accel_y = accel_y;
             m_accel_z = accel_z;
-            m_compAngleX = compAngleX * rad_to_deg;
-            m_compAngleY = compAngleY * rad_to_deg;
-            m_accelAngleX = accelAngleX * rad_to_deg;
-            m_accelAngleY = accelAngleY * rad_to_deg;
-            m_average_gyro_rate_x += gyro_rate_x;
-            m_average_gyro_rate_y += gyro_rate_y;
-            m_average_gyro_rate_z += gyro_rate_z;
+            m_compAngleX = Math.toDegrees(compAngleX);
+            m_compAngleY = Math.toDegrees(compAngleY);
+            m_accelAngleX = Math.toDegrees(accelAngleX);
+            m_accelAngleY = Math.toDegrees(accelAngleY);
           }
           m_first_run = false;
         }
-
-        // The inverse of data to read divided by dataset length, his is the number of iterations
-        // of the for loop inverted (so multiplication can be used instead of division)
-        double invTotalIterations = (double) dataset_len / data_to_read;
-        m_average_gyro_rate_x *= invTotalIterations;
-        m_average_gyro_rate_y *= invTotalIterations;
-        m_average_gyro_rate_z *= invTotalIterations;
       } else {
         m_thread_idle = true;
-        data_count = 0;
-        data_remainder = 0;
-        data_to_read = 0;
         previous_timestamp = 0.0;
-        delta_angle_x = 0.0;
-        delta_angle_y = 0.0;
-        delta_angle_z = 0.0;
-        gyro_rate_x = 0.0;
-        gyro_rate_y = 0.0;
-        gyro_rate_z = 0.0;
-        accel_x = 0.0;
-        accel_y = 0.0;
-        accel_z = 0.0;
-        gyro_rate_x_si = 0.0;
-        gyro_rate_y_si = 0.0;
-        gyro_rate_z_si = 0.0;
-        accel_x_si = 0.0;
-        accel_y_si = 0.0;
-        accel_z_si = 0.0;
         compAngleX = 0.0;
         compAngleY = 0.0;
-        accelAngleX = 0.0;
-        accelAngleY = 0.0;
       }
     }
   }
 
-  /**
-   * @param compAngle
-   * @param accAngle
-   * @return
-   */
   private double formatFastConverge(double compAngle, double accAngle) {
     if (compAngle > accAngle + Math.PI) {
       compAngle = compAngle - 2.0 * Math.PI;
@@ -995,25 +804,6 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
     return compAngle;
   }
 
-  /**
-   * @param compAngle
-   * @return
-   */
-  private double formatRange0to2PI(double compAngle) {
-    while (compAngle >= 2 * Math.PI) {
-      compAngle = compAngle - 2.0 * Math.PI;
-    }
-    while (compAngle < 0.0) {
-      compAngle = compAngle + 2.0 * Math.PI;
-    }
-    return compAngle;
-  }
-
-  /**
-   * @param accelAngle
-   * @param accelZ
-   * @return
-   */
   private double formatAccelRange(double accelAngle, double accelZ) {
     if (accelZ < 0.0) {
       accelAngle = Math.PI - accelAngle;
@@ -1023,20 +813,10 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
     return accelAngle;
   }
 
-  /**
-   * @param compAngle
-   * @param accelAngle
-   * @param omega
-   * @return
-   */
   private double compFilterProcess(double compAngle, double accelAngle, double omega) {
     compAngle = formatFastConverge(compAngle, accelAngle);
     compAngle = m_alpha * (compAngle + omega * m_dt) + (1.0 - m_alpha) * accelAngle;
-    compAngle = formatRange0to2PI(compAngle);
-    if (compAngle > Math.PI) {
-      compAngle = compAngle - 2.0 * Math.PI;
-    }
-    return compAngle;
+    return MathUtil.angleModulus(compAngle);
   }
 
   /**
@@ -1159,27 +939,7 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
    * @return The Yaw axis angle in degrees (CCW positive).
    */
   public synchronized double getAngle() {
-    return switch (m_yaw_axis) {
-      case kX -> {
-        if (m_simGyroAngleX != null) {
-          yield m_simGyroAngleX.get();
-        }
-        yield m_integ_angle_x;
-      }
-      case kY -> {
-        if (m_simGyroAngleY != null) {
-          yield m_simGyroAngleY.get();
-        }
-        yield m_integ_angle_y;
-      }
-      case kZ -> {
-        if (m_simGyroAngleZ != null) {
-          yield m_simGyroAngleZ.get();
-        }
-        yield m_integ_angle_z;
-      }
-      default -> 0.0;
-    };
+    return getAngle(m_yaw_axis);
   }
 
   /**
@@ -1226,27 +986,7 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
    * @return Yaw axis angular rate in degrees per second (CCW positive).
    */
   public synchronized double getRate() {
-    return switch (m_yaw_axis) {
-      case kX -> {
-        if (m_simGyroRateX != null) {
-          yield m_simGyroRateX.get();
-        }
-        yield m_gyro_rate_x;
-      }
-      case kY -> {
-        if (m_simGyroRateY != null) {
-          yield m_simGyroRateY.get();
-        }
-        yield m_gyro_rate_y;
-      }
-      case kZ -> {
-        if (m_simGyroRateZ != null) {
-          yield m_simGyroRateZ.get();
-        }
-        yield m_gyro_rate_z;
-      }
-      default -> 0.0;
-    };
+    return getRate(m_yaw_axis);
   }
 
   /**