Make the IMU communications more reliable

This hooks up support for the reset line and forces the chip select line
to actually change state.

Change-Id: I41427334789e373728cbb9b4327f17540aadbc33
diff --git a/frc971/wpilib/ADIS16448.cc b/frc971/wpilib/ADIS16448.cc
index d551667..e539828 100644
--- a/frc971/wpilib/ADIS16448.cc
+++ b/frc971/wpilib/ADIS16448.cc
@@ -27,6 +27,10 @@
       LOG(INFO, "SPI::Transaction of %zd bytes failed\n", size);
       return false;
     case size:
+      if (dummy_spi_) {
+        uint8_t dummy_send, dummy_receive;
+        dummy_spi_->Transaction(&dummy_send, &dummy_receive, 1);
+      }
       return true;
     default:
       LOG(FATAL, "SPI::Transaction returned something weird\n");
@@ -115,8 +119,9 @@
 
 ADIS16448::ADIS16448(SPI::Port port, DigitalInput *dio1)
     : spi_(new SPI(port)), dio1_(dio1) {
-  // 1MHz is the maximum supported for burst reads.
-  spi_->SetClockRate(1e6);
+  // 1MHz is the maximum supported for burst reads, but we
+  // want to go slower to hopefully make it more reliable.
+  spi_->SetClockRate(1e5);
   spi_->SetChipSelectActiveLow();
   spi_->SetClockActiveLow();
   spi_->SetSampleDataOnFalling();
@@ -126,6 +131,39 @@
   dio1_->SetUpSourceEdge(true, false);
 }
 
+void ADIS16448::SetDummySPI(SPI::Port port) {
+  dummy_spi_.reset(new SPI(port));
+  // Pick the same settings here in case the roboRIO decides to try something
+  // stupid when switching.
+  if (dummy_spi_) {
+    dummy_spi_->SetClockRate(1e5);
+    dummy_spi_->SetChipSelectActiveLow();
+    dummy_spi_->SetClockActiveLow();
+    dummy_spi_->SetSampleDataOnFalling();
+    dummy_spi_->SetMSBFirst();
+  }
+}
+
+void ADIS16448::InitializeUntilSuccessful() {
+  while (run_ && !Initialize()) {
+    if (reset_) {
+      reset_->Set(false);
+      // Datasheet says this needs to be at least 10 us long, so 10 ms is
+      // plenty.
+      ::std::this_thread::sleep_for(::std::chrono::milliseconds(10));
+      reset_->Set(true);
+      // Datasheet says this takes 90 ms typically, and we want to give it
+      // plenty of margin.
+      ::std::this_thread::sleep_for(::std::chrono::milliseconds(150));
+    } else {
+      ::std::this_thread::sleep_for(::std::chrono::milliseconds(50));
+    }
+  }
+  LOG(INFO, "IMU initialized successfully\n");
+
+  ::aos::SetCurrentThreadRealtimePriority(33);
+}
+
 void ADIS16448::operator()() {
   // NI's SPI driver defaults to SCHED_OTHER.  Find it's PID with ps, and change
   // it to a RT priority of 33.
@@ -135,13 +173,7 @@
 
   ::aos::SetCurrentThreadName("IMU");
 
-  // Try to initialize repeatedly as long as we're supposed to be running.
-  while (run_ && !Initialize()) {
-    ::std::this_thread::sleep_for(::std::chrono::milliseconds(50));
-  }
-  LOG(INFO, "IMU initialized successfully\n");
-
-  ::aos::SetCurrentThreadRealtimePriority(33);
+  InitializeUntilSuccessful();
 
   // Rounded to approximate the 204.8 Hz.
   constexpr size_t kImuSendRate = 205;
@@ -157,6 +189,7 @@
           dio1_->WaitForInterrupt(0.1, !got_an_interrupt);
       if (result == InterruptableSensorBase::kTimeout) {
         LOG(WARNING, "IMU read timed out\n");
+        InitializeUntilSuccessful();
         continue;
       }
     }
@@ -187,6 +220,7 @@
       if (received_crc != calculated_crc) {
         LOG(WARNING, "received CRC %" PRIx16 " but calculated %" PRIx16 "\n",
             received_crc, calculated_crc);
+        InitializeUntilSuccessful();
         continue;
       }
     }
@@ -194,7 +228,10 @@
     {
       uint16_t diag_stat;
       memcpy(&diag_stat, &to_receive[2], 2);
-      if (!CheckDiagStatValue(diag_stat)) continue;
+      if (!CheckDiagStatValue(diag_stat)) {
+        InitializeUntilSuccessful();
+        continue;
+      }
     }
 
     auto message = imu_values.MakeMessage();