diff --git a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp index 2ae6d5dd503..a0c6a4045a1 100644 --- a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp +++ b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp @@ -105,6 +105,10 @@ ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port, ConfigCalTime(cal_time); + m_spi = new SPI(m_spi_port); + m_spi->SetClockRate(1000000); + m_spi->SetMode(frc::SPI::Mode::kMode3); + m_spi->SetChipSelectActiveLow(); // Configure standard SPI if (!SwitchToStandardSPI()) { return; @@ -180,6 +184,7 @@ ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port, "required!"); } + m_auto_interrupt = new DigitalInput(10); // Configure and enable auto SPI if (!SwitchToAutoSPI()) { return; @@ -353,7 +358,7 @@ bool ADIS16448_IMU::SwitchToStandardSPI() { Wait(10_ms); } // Maybe we're in auto SPI mode? If so, kill auto SPI, and then SPI. - if (m_spi != nullptr && m_auto_configured) { + if (m_auto_configured) { m_spi->StopAuto(); // We need to get rid of all the garbage left in the auto SPI buffer after // stopping it. @@ -372,13 +377,6 @@ bool ADIS16448_IMU::SwitchToStandardSPI() { } } } - // There doesn't seem to be a SPI port active. Let's try to set one up - if (m_spi == nullptr) { - m_spi = new SPI(m_spi_port); - 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); @@ -420,15 +418,6 @@ void ADIS16448_IMU::InitOffsetBuffer(int size) { *are hard-coded to work only with the ADIS16448 IMU. **/ bool ADIS16448_IMU::SwitchToAutoSPI() { - // No SPI port has been set up. Go set one up first. - if (m_spi == nullptr && !SwitchToStandardSPI()) { - REPORT_ERROR("Failed to start/restart auto SPI"); - return false; - } - // Only set up the interrupt if needed. - if (m_auto_interrupt == nullptr) { - m_auto_interrupt = new DigitalInput(10); - } // The auto SPI controller gets angry if you try to set up two instances on // one bus. if (!m_auto_configured) { diff --git a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp index 187bbb57aec..4decdcb239d 100644 --- a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp +++ b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp @@ -124,6 +124,10 @@ ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis, m_reset_in = new DigitalInput(27); // Set SPI CS2 (IMU RST) high Wait(500_ms); // Wait for reset to complete + m_spi = new SPI(m_spi_port); + m_spi->SetClockRate(2000000); + m_spi->SetMode(frc::SPI::Mode::kMode3); + m_spi->SetChipSelectActiveLow(); // Configure standard SPI if (!SwitchToStandardSPI()) { return; @@ -188,6 +192,7 @@ ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis, // Write offset calibration command to IMU WriteRegister(GLOB_CMD, 0x0001); + m_auto_interrupt = new DigitalInput(26); // Configure and enable auto SPI if (!SwitchToAutoSPI()) { return; @@ -332,7 +337,7 @@ bool ADIS16470_IMU::SwitchToStandardSPI() { Wait(10_ms); } // Maybe we're in auto SPI mode? If so, kill auto SPI, and then SPI. - if (m_spi != nullptr && m_auto_configured) { + if (m_auto_configured) { m_spi->StopAuto(); // We need to get rid of all the garbage left in the auto SPI buffer after // stopping it. @@ -350,13 +355,6 @@ bool ADIS16470_IMU::SwitchToStandardSPI() { } } } - // There doesn't seem to be a SPI port active. Let's try to set one up - if (m_spi == nullptr) { - m_spi = new SPI(m_spi_port); - 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); @@ -385,16 +383,6 @@ bool ADIS16470_IMU::SwitchToStandardSPI() { *are hard-coded to work only with the ADIS16470 IMU. **/ bool ADIS16470_IMU::SwitchToAutoSPI() { - // No SPI port has been set up. Go set one up first. - if (m_spi == nullptr && !SwitchToStandardSPI()) { - REPORT_ERROR("Failed to start/restart auto SPI"); - return false; - } - - // Only set up the interrupt if needed. - if (m_auto_interrupt == nullptr) { - m_auto_interrupt = new DigitalInput(26); - } // The auto SPI controller gets angry if you try to set up two instances on // one bus. if (!m_auto_configured) { 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 38c0bfbab33..8843a9b1e34 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 @@ -269,6 +269,10 @@ public ADIS16448_IMU(final IMUAxis yaw_axis, SPI.Port port, CalibrationTime cal_ configCalTime(cal_time); + m_spi = new SPI(m_spi_port); + m_spi.setClockRate(1000000); + m_spi.setMode(SPI.Mode.kMode3); + m_spi.setChipSelectActiveLow(); if (!switchToStandardSPI()) { return; } @@ -337,6 +341,8 @@ public ADIS16448_IMU(final IMUAxis yaw_axis, SPI.Port port, CalibrationTime cal_ "ADIS16448: Flash and RAM configuration consistent. No flash update required!", false); } + // Set up the interrupt + m_auto_interrupt = new DigitalInput(10); // MXP DIO0 // Configure standard SPI if (!switchToAutoSPI()) { return; @@ -396,7 +402,7 @@ private boolean switchToStandardSPI() { } System.out.println("Paused the IMU processing thread successfully!"); // Maybe we're in auto SPI mode? If so, kill auto SPI, and then SPI. - if (m_spi != null && m_auto_configured) { + if (m_auto_configured) { m_spi.stopAuto(); // We need to get rid of all the garbage left in the auto SPI buffer after // stopping it. @@ -417,14 +423,6 @@ private boolean switchToStandardSPI() { System.out.println("Paused auto SPI successfully."); } } - // There doesn't seem to be a SPI port active. Let's try to set one up - if (m_spi == null) { - System.out.println("Setting up a new SPI port."); - m_spi = new SPI(m_spi_port); - 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) { @@ -436,15 +434,6 @@ private boolean switchToStandardSPI() { } boolean switchToAutoSPI() { - // No SPI port has been set up. Go set one up first. - if (m_spi == null && !switchToStandardSPI()) { - DriverStation.reportError("Failed to start/restart auto SPI", false); - return false; - } - // Only set up the interrupt if needed. - if (m_auto_interrupt == null) { - m_auto_interrupt = new DigitalInput(10); // MXP DIO0 - } // The auto SPI controller gets angry if you try to set up two instances on one // bus. if (!m_auto_configured) { 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 e3528e6d137..e158cb1369c 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 @@ -337,6 +337,10 @@ public ADIS16470_IMU( m_reset_in = new DigitalInput(27); // Set SPI CS2 (IMU RST) high Timer.delay(0.25); // Wait for reset to complete + m_spi = new SPI(m_spi_port); + m_spi.setClockRate(2000000); + m_spi.setMode(SPI.Mode.kMode3); + m_spi.setChipSelectActiveLow(); if (!switchToStandardSPI()) { return; } @@ -397,6 +401,8 @@ public ADIS16470_IMU( // Write offset calibration command to IMU writeRegister(GLOB_CMD, 0x0001); + // Configure interrupt on SPI CS1 + m_auto_interrupt = new DigitalInput(26); // Configure and enable auto SPI if (!switchToAutoSPI()) { return; @@ -452,7 +458,7 @@ private boolean switchToStandardSPI() { } System.out.println("Paused the IMU processing thread successfully!"); // Maybe we're in auto SPI mode? If so, kill auto SPI, and then SPI. - if (m_spi != null && m_auto_configured) { + if (m_auto_configured) { m_spi.stopAuto(); // We need to get rid of all the garbage left in the auto SPI buffer after // stopping it. @@ -471,14 +477,6 @@ private boolean switchToStandardSPI() { System.out.println("Paused auto SPI successfully."); } } - // There doesn't seem to be a SPI port active. Let's try to set one up - if (m_spi == null) { - System.out.println("Setting up a new SPI port."); - m_spi = new SPI(m_spi_port); - 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) { @@ -495,16 +493,6 @@ private boolean switchToStandardSPI() { * @return True if successful, false otherwise. */ boolean switchToAutoSPI() { - // No SPI port has been set up. Go set one up first. - if (m_spi == null && !switchToStandardSPI()) { - DriverStation.reportError("Failed to start/restart auto SPI", false); - return false; - } - // Only set up the interrupt if needed. - if (m_auto_interrupt == null) { - // Configure interrupt on SPI CS1 - m_auto_interrupt = new DigitalInput(26); - } // The auto SPI controller gets angry if you try to set up two instances on one // bus. if (!m_auto_configured) {