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();