From 9c933f88a90f2b45bdcbdb14ad9ef3fef4aceeda Mon Sep 17 00:00:00 2001
From: Gold856 <117957790+Gold856@users.noreply.github.com>
Date: Wed, 5 Jun 2024 17:12:53 -0400
Subject: [PATCH] Use hypot function

---
 wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp          | 10 ++++------
 wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp          | 10 ++++------
 .../main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java | 10 ++--------
 .../main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java |  8 ++------
 4 files changed, 12 insertions(+), 26 deletions(-)

diff --git a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp
index de74891849..4f4d9c0acb 100644
--- a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp
+++ b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp
@@ -670,12 +670,10 @@ void ADIS16448_IMU::Acquire() {
           // Calculate alpha for use with the complementary filter
           m_alpha = kTau / (kTau + 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)));
+          double accelAngleX =
+              atan2f(-accel_x_si, std::hypotf(accel_y_si, -accel_z_si));
+          double accelAngleY =
+              atan2f(accel_y_si, std::hypotf(-accel_x_si, -accel_z_si));
           // Calculate complementary filter
           if (m_first_run) {
             compAngleX = accelAngleX;
diff --git a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp
index 08ec99ee86..f3eeb35403 100644
--- a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp
+++ b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp
@@ -698,12 +698,10 @@ void ADIS16470_IMU::Acquire() {
         m_alpha = kTau / (kTau + 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)));
+        double accelAngleX =
+            atan2f(accel_x_si, std::hypotf(accel_y_si, accel_z_si));
+        double accelAngleY =
+            atan2f(accel_y_si, std::hypotf(accel_x_si, accel_z_si));
         if (m_first_run) {
           compAngleX = accelAngleX;
           compAngleY = accelAngleY;
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 26ef204053..49a59bb195 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
@@ -760,14 +760,8 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
             // Calculate alpha for use with the complementary filter
             m_alpha = kTau / (kTau + 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)));
+            double accelAngleX = Math.atan2(-accel_x_si, Math.hypot(accel_y_si, -accel_z_si));
+            double accelAngleY = Math.atan2(accel_y_si, Math.hypot(-accel_x_si, -accel_z_si));
             // Calculate complementary filter
             if (m_first_run) {
               compAngleX = accelAngleX;
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 574ea17a32..2655284ee2 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
@@ -747,12 +747,8 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
           m_alpha = kTau / (kTau + 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)));
+          double accelAngleX = Math.atan2(accel_x_si, Math.hypot(accel_y_si, accel_z_si));
+          double accelAngleY = Math.atan2(accel_y_si, Math.hypot(accel_x_si, accel_z_si));
           if (m_first_run) {
             compAngleX = accelAngleX;
             compAngleY = accelAngleY;