[wpilib] ADIS IMUs: Add back null checks (#7117)

* Revert "Move creation of objects to ctor"

This reverts commit b1d8001652.

* [wpilib] ADIS IMUs: Add back null checks

wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp

@@ -99,11 +99,6 @@ 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;
     }
@@ -178,8 +173,6 @@ 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;
     }
@@ -352,7 +345,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_auto_configured) {
+    if (m_spi != nullptr && m_auto_configured) {
       m_spi->StopAuto();
       // We need to get rid of all the garbage left in the auto SPI buffer after
       // stopping it.
@@ -371,6 +364,12 @@ bool ADIS16448_IMU::SwitchToStandardSPI() {
       }
     }
   }
+  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);
@@ -412,6 +411,15 @@ 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) {

wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp

@@ -118,10 +118,6 @@ 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;
@@ -186,7 +182,6 @@ 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;
@@ -331,7 +326,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_auto_configured) {
+    if (m_spi != nullptr && m_auto_configured) {
       m_spi->StopAuto();
       // We need to get rid of all the garbage left in the auto SPI buffer after
       // stopping it.
@@ -349,6 +344,13 @@ 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);
@@ -377,6 +379,16 @@ 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) {

wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java

@@ -270,10 +270,6 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
 
       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;
       }
@@ -342,9 +338,6 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
             "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;
       }
@@ -403,7 +396,7 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
       }
       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_auto_configured) {
+      if (m_spi != null && m_auto_configured) {
         m_spi.stopAuto();
         // We need to get rid of all the garbage left in the auto SPI buffer after
         // stopping it.
@@ -424,6 +417,12 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
         System.out.println("Paused auto SPI successfully.");
       }
     }
+    if (m_spi == null) {
+      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) {
@@ -435,6 +434,15 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
   }
 
   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) {

wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java

@@ -338,10 +338,6 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
       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;
       }
@@ -402,9 +398,6 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
       // 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;
       }
@@ -459,7 +452,7 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
       }
       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_auto_configured) {
+      if (m_spi != null && m_auto_configured) {
         m_spi.stopAuto();
         // We need to get rid of all the garbage left in the auto SPI buffer after
         // stopping it.
@@ -478,6 +471,12 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
         System.out.println("Paused auto SPI successfully.");
       }
     }
+    if (m_spi == null) {
+      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) {
@@ -494,6 +493,16 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
    * @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) {