From 0f1c62f2ce36945c56a7fe13c78cc8dd17a9a466 Mon Sep 17 00:00:00 2001 From: Gold856 <117957790+Gold856@users.noreply.github.com> Date: Wed, 5 Jun 2024 23:11:11 -0400 Subject: [PATCH] Clean up numeric constants --- wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp | 20 ++++++++--------- wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp | 20 ++++++++--------- .../main/native/include/frc/ADIS16448_IMU.h | 6 ++--- .../main/native/include/frc/ADIS16470_IMU.h | 6 ++--- .../edu/wpi/first/wpilibj/ADIS16448_IMU.java | 14 ++++++------ .../edu/wpi/first/wpilibj/ADIS16470_IMU.java | 22 +++++++++---------- 6 files changed, 44 insertions(+), 44 deletions(-) diff --git a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp index 4f4d9c0acb..bf90157094 100644 --- a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp +++ b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp @@ -659,12 +659,12 @@ void ADIS16448_IMU::Acquire() { double temp = BuffToShort(&buffer[i + 25]) * 0.07386 + 31.0; // Convert scaled sensor data to SI units - 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; + double gyro_rate_x_si = gyro_rate_x * kDegToRad; + double gyro_rate_y_si = gyro_rate_y * kDegToRad; + // double gyro_rate_z_si = gyro_rate_z * kDegToRad; + double accel_x_si = accel_x * kGrav; + double accel_y_si = accel_y * kGrav; + double accel_z_si = accel_z * kGrav; // Store timestamp for next iteration previous_timestamp = buffer[i]; // Calculate alpha for use with the complementary filter @@ -718,10 +718,10 @@ void ADIS16448_IMU::Acquire() { 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 = compAngleX * kRadToDeg; + m_compAngleY = compAngleY * kRadToDeg; + m_accelAngleX = accelAngleX * kRadToDeg; + m_accelAngleY = accelAngleY * kRadToDeg; // Accumulate gyro for angle integration and publish to global // variables m_integ_gyro_angle_x += diff --git a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp index f3eeb35403..f7b823503c 100644 --- a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp +++ b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp @@ -685,12 +685,12 @@ void ADIS16470_IMU::Acquire() { double accel_z = BuffToShort(&buffer[i + 25]) / 800.0; // Convert scaled sensor data to SI units - 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; + double gyro_rate_x_si = gyro_rate_x * kDegToRad; + double gyro_rate_y_si = gyro_rate_y * kDegToRad; + // double gyro_rate_z_si = gyro_rate_z * kDegToRad; + double accel_x_si = accel_x * kGrav; + double accel_y_si = accel_y * kGrav; + double accel_z_si = accel_z * kGrav; // Store timestamp for next iteration previous_timestamp = buffer[i]; @@ -734,10 +734,10 @@ void ADIS16470_IMU::Acquire() { 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_compAngleX = compAngleX * kRadToDeg; + m_compAngleY = compAngleY * kRadToDeg; + m_accelAngleX = accelAngleX * kRadToDeg; + m_accelAngleY = accelAngleY * kRadToDeg; } m_first_run = false; } diff --git a/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h b/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h index fc4a844ef2..71d52b8160 100644 --- a/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h +++ b/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h @@ -338,9 +338,9 @@ class ADIS16448_IMU : public wpi::Sendable, static constexpr uint8_t SERIAL_NUM = 0x58; // Lot-specific serial number /** @brief ADIS16448 Static Constants */ - static constexpr double rad_to_deg = 57.2957795; - static constexpr double deg_to_rad = 0.0174532; - static constexpr double grav = 9.81; + static constexpr double kRadToDeg = 57.2957795; + static constexpr double kDegToRad = 0.0174532; + static constexpr double kGrav = 9.81; /** @brief struct to store offset data */ struct OffsetData { diff --git a/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h index 6ab2c1871c..a55800c416 100644 --- a/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h +++ b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h @@ -420,9 +420,9 @@ class ADIS16470_IMU : public wpi::Sendable, Y_ACCL_OUT, FLASH_CNT, Z_ACCL_OUT, FLASH_CNT}; static constexpr double delta_angle_sf = 2160.0 / 2147483648.0; - static constexpr double rad_to_deg = 57.2957795; - static constexpr double deg_to_rad = 0.0174532; - static constexpr double grav = 9.81; + static constexpr double kRadToDeg = 57.2957795; + static constexpr double kDegToRad = 0.0174532; + static constexpr double kGrav = 9.81; // Resources DigitalInput* m_reset_in = nullptr; 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 49a59bb195..882e22e90f 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 @@ -118,7 +118,7 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { kZ } - private static final double grav = 9.81; + private static final double kGrav = 9.81; // User-specified yaw axis private IMUAxis m_yaw_axis; @@ -752,9 +752,9 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { 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; + double accel_x_si = accel_x * kGrav; + double accel_y_si = accel_y * kGrav; + double accel_z_si = accel_z * kGrav; // Store timestamp for next iteration previous_timestamp = buffer[i]; // Calculate alpha for use with the complementary filter @@ -963,7 +963,7 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { if (m_simAccelX != null) { return m_simAccelX.get(); } - return m_accel_x * 9.81; + return m_accel_x * kGrav; } /** @@ -975,7 +975,7 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { if (m_simAccelY != null) { return m_simAccelY.get(); } - return m_accel_y * 9.81; + return m_accel_y * kGrav; } /** @@ -987,7 +987,7 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { if (m_simAccelZ != null) { return m_simAccelZ.get(); } - return m_accel_z * 9.81; + return m_accel_z * kGrav; } /** 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 2655284ee2..51ea9d7154 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 @@ -181,8 +181,8 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { } // Static Constants - private static final double delta_angle_sf = 2160.0 / 2147483648.0; // 2160 / (2^31) - private static final double grav = 9.81; + private static final double kDeltaAngleSf = 2160.0 / 2147483648.0; // 2160 / (2^31) + private static final double kGrav = 9.81; // User-specified axes private IMUAxis m_yaw_axis; @@ -713,15 +713,15 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { 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 + * kDeltaAngleSf / elapsed_time; double delta_angle_y = toInt(buffer[i + 7], buffer[i + 8], buffer[i + 9], buffer[i + 10]) - * delta_angle_sf + * kDeltaAngleSf / elapsed_time; double delta_angle_z = toInt(buffer[i + 11], buffer[i + 12], buffer[i + 13], buffer[i + 14]) - * delta_angle_sf + * kDeltaAngleSf / elapsed_time; double gyro_rate_x = toShort(buffer[i + 15], buffer[i + 16]) / 10.0; @@ -737,9 +737,9 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { 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; + double accel_x_si = accel_x * kGrav; + double accel_y_si = accel_y * kGrav; + double accel_z_si = accel_z * kGrav; // Store timestamp for next iteration previous_timestamp = buffer[i]; @@ -1019,7 +1019,7 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { * @return The acceleration in the X axis in meters per second squared. */ public synchronized double getAccelX() { - return m_accel_x * 9.81; + return m_accel_x * kGrav; } /** @@ -1028,7 +1028,7 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { * @return The acceleration in the Y axis in meters per second squared. */ public synchronized double getAccelY() { - return m_accel_y * 9.81; + return m_accel_y * kGrav; } /** @@ -1037,7 +1037,7 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { * @return The acceleration in the Z axis in meters per second squared. */ public synchronized double getAccelZ() { - return m_accel_z * 9.81; + return m_accel_z * kGrav; } /**