diff --git a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp index ef6cdb87ed..2ae6d5dd50 100644 --- a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp +++ b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp @@ -378,29 +378,16 @@ bool ADIS16448_IMU::SwitchToStandardSPI() { m_spi->SetClockRate(1000000); m_spi->SetMode(frc::SPI::Mode::kMode3); m_spi->SetChipSelectActiveLow(); - ReadRegister(PROD_ID); // Dummy read - - // Validate the product ID - uint16_t prod_id = ReadRegister(PROD_ID); - if (prod_id != 16448) { - REPORT_ERROR("Could not find ADIS16448!"); - Close(); - return false; - } - return true; - } else { - // Maybe the SPI port is active, but not in auto SPI mode? Try to read the - // product ID. - ReadRegister(PROD_ID); // Dummy read - uint16_t prod_id = ReadRegister(PROD_ID); - if (prod_id != 16448) { - REPORT_ERROR("Could not find ADIS16448!"); - Close(); - return false; - } else { - return true; - } } + ReadRegister(PROD_ID); // Dummy read + // Validate the product ID + uint16_t prod_id = ReadRegister(PROD_ID); + if (prod_id != 16448) { + REPORT_ERROR("Could not find ADIS16448!"); + Close(); + return false; + } + return true; } void ADIS16448_IMU::InitOffsetBuffer(int size) { diff --git a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp index f7b823503c..187bbb57ae 100644 --- a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp +++ b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp @@ -356,29 +356,16 @@ bool ADIS16470_IMU::SwitchToStandardSPI() { m_spi->SetClockRate(2000000); m_spi->SetMode(frc::SPI::Mode::kMode3); m_spi->SetChipSelectActiveLow(); - ReadRegister(PROD_ID); // Dummy read - - // Validate the product ID - uint16_t prod_id = ReadRegister(PROD_ID); - if (prod_id != 16982 && prod_id != 16470) { - REPORT_ERROR("Could not find ADIS16470!"); - Close(); - return false; - } - return true; - } else { - // Maybe the SPI port is active, but not in auto SPI mode? Try to read the - // product ID. - ReadRegister(PROD_ID); // Dummy read - uint16_t prod_id = ReadRegister(PROD_ID); - if (prod_id != 16982 && prod_id != 16470) { - REPORT_ERROR("Could not find ADIS16470!"); - Close(); - return false; - } else { - return true; - } } + ReadRegister(PROD_ID); // Dummy read + // Validate the product ID + uint16_t prod_id = ReadRegister(PROD_ID); + if (prod_id != 16982 && prod_id != 16470) { + REPORT_ERROR("Could not find ADIS16470!"); + Close(); + return false; + } + return true; } /** 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 0c375be361..38c0bfbab3 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 @@ -424,27 +424,15 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable { m_spi.setClockRate(1000000); m_spi.setMode(SPI.Mode.kMode3); m_spi.setChipSelectActiveLow(); - readRegister(PROD_ID); // Dummy read - - // Validate the product ID - if (readRegister(PROD_ID) != 16448) { - DriverStation.reportError("Could not find ADIS16448", false); - close(); - return false; - } - return true; - } else { - // Maybe the SPI port is active, but not in auto SPI mode? Try to read the - // product ID. - readRegister(PROD_ID); // dummy read - if (readRegister(PROD_ID) != 16448) { - DriverStation.reportError("Could not find an ADIS16448", false); - close(); - return false; - } else { - return true; - } } + readRegister(PROD_ID); // Dummy read + // Validate the product ID + if (readRegister(PROD_ID) != 16448) { + DriverStation.reportError("Could not find ADIS16448", false); + close(); + return false; + } + return true; } boolean switchToAutoSPI() { 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 20913d50bc..e3528e6d13 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 @@ -478,27 +478,15 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { m_spi.setClockRate(2000000); m_spi.setMode(SPI.Mode.kMode3); m_spi.setChipSelectActiveLow(); - readRegister(PROD_ID); // Dummy read - - // Validate the product ID - if (readRegister(PROD_ID) != 16982) { - DriverStation.reportError("Could not find ADIS16470", false); - close(); - return false; - } - return true; - } else { - // Maybe the SPI port is active, but not in auto SPI mode? Try to read the - // product ID. - readRegister(PROD_ID); // dummy read - if (readRegister(PROD_ID) != 16982) { - DriverStation.reportError("Could not find an ADIS16470", false); - close(); - return false; - } else { - return true; - } } + readRegister(PROD_ID); // Dummy read + // Validate the product ID + if (readRegister(PROD_ID) != 16982) { + DriverStation.reportError("Could not find an ADIS16470", false); + close(); + return false; + } + return true; } /**