diff --git a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp index 88bb5dc90a..de74891849 100644 --- a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp +++ b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp @@ -668,7 +668,7 @@ void ADIS16448_IMU::Acquire() { // Store timestamp for next iteration previous_timestamp = buffer[i]; // Calculate alpha for use with the complementary filter - m_alpha = 0.5 / (0.5 + m_dt); + m_alpha = kTau / (kTau + m_dt); // Run inclinometer calculations double accelAngleX = atan2f( -accel_x_si, diff --git a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp index 1ff56d27b8..3172bd803b 100644 --- a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp +++ b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp @@ -695,7 +695,7 @@ void ADIS16470_IMU::Acquire() { // Store timestamp for next iteration previous_timestamp = buffer[i]; - m_alpha = 1.0 / (1.0 + m_dt); + m_alpha = kTau / (kTau + m_dt); // Run inclinometer calculations double accelAngleX = atan2f( diff --git a/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h b/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h index 44b17d7ec5..fc4a844ef2 100644 --- a/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h +++ b/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h @@ -383,6 +383,7 @@ class ADIS16448_IMU : public wpi::Sendable, // Complementary filter variables double m_dt, m_alpha = 0.0; + static constexpr double kTau = 0.5; double m_compAngleX, m_compAngleY, m_accelAngleX, m_accelAngleY = 0.0; // vector for storing most recent imu values diff --git a/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h index bdb164fd90..6ab2c1871c 100644 --- a/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h +++ b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h @@ -490,6 +490,7 @@ class ADIS16470_IMU : public wpi::Sendable, // Complementary filter variables double m_dt, m_alpha = 0.0; + static constexpr double kTau = 1.0; double m_compAngleX, m_compAngleY, m_accelAngleX, m_accelAngleY = 0.0; // Complementary filter functions 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 8eeae4a279..a6544eda67 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 @@ -156,6 +156,7 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { // Complementary filter variables private double m_dt = 0.0; private double m_alpha = 0.0; + private static final double kTau = 1.0; private double m_compAngleX = 0.0; private double m_compAngleY = 0.0; private double m_accelAngleX = 0.0; @@ -757,7 +758,7 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { // Store timestamp for next iteration previous_timestamp = buffer[i]; // Calculate alpha for use with the complementary filter - m_alpha = 1.0 / (1.0 + m_dt); + m_alpha = kTau / (kTau + m_dt); // Run inclinometer calculations double accelAngleX = Math.atan2( 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 337c0f2370..14a5630c2f 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 @@ -205,6 +205,7 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { // Complementary filter variables private double m_dt = 0.0; private double m_alpha = 0.0; + private static final double kTau = 1.0; private double m_compAngleX = 0.0; private double m_compAngleY = 0.0; private double m_accelAngleX = 0.0; @@ -743,7 +744,7 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { // Store timestamp for next iteration previous_timestamp = buffer[i]; - m_alpha = 1.0 / (1.0 + m_dt); + m_alpha = kTau / (kTau + m_dt); // Run inclinometer calculations double accelAngleX =