diff --git a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp index 1a1940891b..eb31386746 100644 --- a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp +++ b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp @@ -102,8 +102,8 @@ ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port, DigitalOutput* m_reset_out = new DigitalOutput(18); // Drive MXP DIO8 low Wait(10_ms); // Wait 10ms delete m_reset_out; - new DigitalInput(18); // Set MXP DIO8 high - Wait(500_ms); // Wait 500ms for reset to complete + m_reset_in = new DigitalInput(18); // Set MXP DIO8 high + Wait(500_ms); // Wait 500ms for reset to complete ConfigCalTime(cal_time); @@ -143,7 +143,7 @@ ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port, // TODO: Find what the proper pin is to turn this LED // Drive MXP PWM5 (IMU ready LED) low (active low) - new DigitalOutput(19); + m_status_led = new DigitalOutput(19); } // Report usage and post data to DS @@ -387,6 +387,14 @@ void ADIS16448_IMU::Reset() { } void ADIS16448_IMU::Close() { + if (m_reset_in != nullptr) { + delete m_reset_in; + m_reset_in = nullptr; + } + if (m_status_led != nullptr) { + delete m_status_led; + m_status_led = nullptr; + } if (m_thread_active) { m_thread_active = false; if (m_acquire_task.joinable()) { diff --git a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp index bf7761612e..7127fa82ee 100644 --- a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp +++ b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp @@ -101,8 +101,8 @@ ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, SPI::Port port, new DigitalOutput(27); // Drive SPI CS2 (IMU RST) low Wait(10_ms); // Wait 10ms delete m_reset_out; - new DigitalInput(27); // Set SPI CS2 (IMU RST) high - Wait(500_ms); // Wait 500ms for reset to complete + m_reset_in = new DigitalInput(27); // Set SPI CS2 (IMU RST) high + Wait(500_ms); // Wait 500ms for reset to complete // Configure standard SPI if (!SwitchToStandardSPI()) { @@ -140,7 +140,7 @@ ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, SPI::Port port, REPORT_WARNING("ADIS16470 IMU Successfully Initialized!"); // Drive SPI CS3 (IMU ready LED) low (active low) - new DigitalOutput(28); + m_status_led = new DigitalOutput(28); } // Report usage and post data to DS @@ -441,6 +441,14 @@ void ADIS16470_IMU::Reset() { } void ADIS16470_IMU::Close() { + if (m_reset_in != nullptr) { + delete m_reset_in; + m_reset_in = nullptr; + } + if (m_status_led != nullptr) { + delete m_status_led; + m_status_led = nullptr; + } if (m_thread_active) { m_thread_active = false; if (m_acquire_task.joinable()) { diff --git a/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h b/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h index 0dbfa686db..01b0fecb3f 100644 --- a/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h +++ b/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h @@ -285,6 +285,10 @@ class ADIS16448_IMU : public nt::NTSendable, double gyro_rate_z = 0.0; }; + /** @brief Internal Resources **/ + DigitalInput* m_reset_in; + DigitalOutput* m_status_led; + bool SwitchToStandardSPI(); bool SwitchToAutoSPI(); diff --git a/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h index e6f3ce7909..ada01e2614 100644 --- a/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h +++ b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h @@ -296,6 +296,10 @@ class ADIS16470_IMU : public nt::NTSendable, static constexpr double deg_to_rad = 0.0174532; static constexpr double grav = 9.81; + /** @brief Resources **/ + DigitalInput* m_reset_in; + DigitalOutput* m_status_led; + /** * @brief Switches to standard SPI operation. Primarily used when exiting auto * SPI mode.