diff --git a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp index 72dcbbfd203..0d1697673b3 100644 --- a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp +++ b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp @@ -509,7 +509,7 @@ void ADIS16448_IMU::Reset() { m_integ_gyro_angle_x = 0.0; m_integ_gyro_angle_y = 0.0; m_integ_gyro_angle_z = 0.0; - angleOffset = frc::Rotation3d{}; + m_angleOffset = frc::Rotation3d{}; } void ADIS16448_IMU::Reset(Rotation3d angle) { @@ -517,17 +517,7 @@ void ADIS16448_IMU::Reset(Rotation3d angle) { m_integ_gyro_angle_x = 0.0; m_integ_gyro_angle_y = 0.0; m_integ_gyro_angle_z = 0.0; - angleOffset = -angle; - - if (m_simGyroAngleX) { - m_simGyroAngleX.Set(0.0); - } - if (m_simGyroAngleY) { - m_simGyroAngleY.Set(0.0); - } - if (m_simGyroAngleZ) { - m_simGyroAngleZ.Set(0.0); - } + m_angleOffset = angle; } void ADIS16448_IMU::Close() { @@ -900,15 +890,15 @@ frc::Rotation3d ADIS16448_IMU::GetGyroOrientation() const { } units::degree_t ADIS16448_IMU::GetGyroAngleX() const { - return (GetGyroOrientation() - m_angleOffset).X(); + return (GetGyroOrientation() + m_angleOffset).X(); } units::degree_t ADIS16448_IMU::GetGyroAngleY() const { - return (GetGyroOrientation() - m_angleOffset).Y(); + return (GetGyroOrientation() + m_angleOffset).Y(); } units::degree_t ADIS16448_IMU::GetGyroAngleZ() const { - return (GetGyroOrientation() - m_angleOffset).Z(); + return (GetGyroOrientation() + m_angleOffset).Z(); } units::degrees_per_second_t ADIS16448_IMU::GetGyroRateX() const { diff --git a/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h b/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h index f4a1ff22cfb..e1f7d598b09 100644 --- a/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h +++ b/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h @@ -362,7 +362,6 @@ class ADIS16448_IMU : public wpi::Sendable, /** @brief Internal Resources **/ DigitalInput* m_reset_in = nullptr; DigitalOutput* m_status_led = nullptr; - Rotation3d angleOffset = Rotation3d{0_deg, 0_deg, 0_deg}; bool SwitchToStandardSPI(); diff --git a/wpilibc/src/test/native/cpp/simulation/ADIS16488_IMUSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/ADIS16488_IMUSimTest.cpp index 7b674d21e8e..35810b4df01 100644 --- a/wpilibc/src/test/native/cpp/simulation/ADIS16488_IMUSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/ADIS16488_IMUSimTest.cpp @@ -4,54 +4,54 @@ #include #include +#include +#include "frc/ADIS16448_IMU.h" #include "frc/geometry/Rotation3d.h" #include "frc/simulation/ADIS16448_IMUSim.h" -#include "frc/ADIS16448_IMU.h" -#include "units/angle.h" namespace frc::sim { - TEST(ADIS16448_IMUSimTest, SetAttributes) { - HAL_Initialize(500, 0); - - ADIS16448_IMU gyro; - ADIS16448_IMUSim sim{gyro}; - - EXPECT_EQ(0, gyro.GetAngle().value()); - EXPECT_EQ(0, gyro.GetGyroAngleX().value()); - EXPECT_EQ(0, gyro.GetGyroAngleY().value()); - EXPECT_EQ(0, gyro.GetGyroAngleZ().value()); - EXPECT_EQ(0, gyro.GetRate().value()); - - constexpr units::degree_t TEST_ANGLE{123.456}; - constexpr units::degrees_per_second_t TEST_RATE{229.3504}; - sim.SetGyroAngleZ(TEST_ANGLE); - sim.SetGyroRateZ(TEST_RATE); - EXPECT_EQ(TEST_ANGLE, gyro.GetAngle()); - EXPECT_EQ(TEST_RATE, gyro.GetGyroRateZ()); - } - - TEST(ADIS16448_IMUSimTest, Offset) { - HAL_Initialize(500, 0); - - ADIS16448_IMU gyro{}; - ADIS16448_IMUSim sim{gyro}; - - frc::Rotation3d OFFSET1{0_deg, 0_deg, 90_deg}; - gyro.Reset(OFFSET1); - EXPECT_EQ(90_deg, gyro.GetAngle()); - EXPECT_EQ(0_deg, gyro.GetGyroAngleX()); - EXPECT_EQ(0_deg, gyro.GetGyroAngleY()); - EXPECT_EQ(90_deg, gyro.GetGyroAngleZ()); - - sim.SetGyroAngleZ(10_deg); - EXPECT_EQ(100_deg, gyro.GetAngle()); - - sim.SetGyroAngleZ(90_deg); - EXPECT_EQ(180_deg, gyro.GetAngle()); - - gyro.Reset(frc::Rotation3d{0_deg,90_deg,90_deg}); - EXPECT_EQ(90_deg, gyro.GetGyroAngleY()); - EXPECT_EQ(90_deg, gyro.GetAngle()); - } -} // namespace frc::sim \ No newline at end of file +TEST(ADIS16448_IMUSimTest, SetAttributes) { + HAL_Initialize(500, 0); + + ADIS16448_IMU gyro; + ADIS16448_IMUSim sim{gyro}; + + EXPECT_EQ(0, gyro.GetAngle().value()); + EXPECT_EQ(0, gyro.GetGyroAngleX().value()); + EXPECT_EQ(0, gyro.GetGyroAngleY().value()); + EXPECT_EQ(0, gyro.GetGyroAngleZ().value()); + EXPECT_EQ(0, gyro.GetRate().value()); + + constexpr units::degree_t TEST_ANGLE{123.456}; + constexpr units::degrees_per_second_t TEST_RATE{229.3504}; + sim.SetGyroAngleZ(TEST_ANGLE); + sim.SetGyroRateZ(TEST_RATE); + EXPECT_EQ(TEST_ANGLE, gyro.GetAngle()); + EXPECT_EQ(TEST_RATE, gyro.GetGyroRateZ()); +} + +TEST(ADIS16448_IMUSimTest, Offset) { + HAL_Initialize(500, 0); + + ADIS16448_IMU gyro{}; + ADIS16448_IMUSim sim{gyro}; + + frc::Rotation3d OFFSET1{0_deg, 0_deg, 90_deg}; + gyro.Reset(OFFSET1); + EXPECT_EQ(90_deg, gyro.GetAngle()); + EXPECT_EQ(0_deg, gyro.GetGyroAngleX()); + EXPECT_EQ(0_deg, gyro.GetGyroAngleY()); + EXPECT_EQ(90_deg, gyro.GetGyroAngleZ()); + + sim.SetGyroAngleZ(10_deg); + EXPECT_EQ(100_deg, gyro.GetAngle()); + + sim.SetGyroAngleZ(90_deg); + EXPECT_EQ(180_deg, gyro.GetAngle()); + + gyro.Reset(frc::Rotation3d{0_deg, 90_deg, 90_deg}); + EXPECT_EQ(90_deg, gyro.GetGyroAngleY()); + EXPECT_EQ(90_deg, gyro.GetAngle()); +} +} // namespace frc::sim