remove extraneous offset field

DeltaDizzy · May 1, 2024 · 17ad277 · 17ad277
1 parent 5c0448d
commit 17ad277
Show file tree

Hide file tree

Showing 3 changed files with 51 additions and 62 deletions.
diff --git a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp
@@ -509,25 +509,15 @@ 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) {
   std::scoped_lock sync(m_mutex);
   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
@@ -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
@@ -4,54 +4,54 @@
 
 #include <gtest/gtest.h>
 #include <hal/HAL.h>
+#include <units/angle.h>
 
+#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
+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