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/aos/linux_code/init.cc b/aos/linux_code/init.cc
index 1136d4d..47c7f35 100644
--- a/aos/linux_code/init.cc
+++ b/aos/linux_code/init.cc
@@ -155,7 +155,15 @@
struct sched_param param;
param.sched_priority = priority;
if (sched_setscheduler(0, SCHED_FIFO, ¶m) == -1) {
- PLOG(FATAL, "sched_setscheduler(0, SCHED_FIFO, %d) failed", priority);
+ PLOG(FATAL, "sched_setscheduler(0, SCHED_FIFO, %d) failed\n", priority);
+ }
+}
+
+void UnsetCurrentThreadRealtimePriority() {
+ struct sched_param param;
+ param.sched_priority = 0;
+ if (sched_setscheduler(0, SCHED_OTHER, ¶m) == -1) {
+ PLOG(FATAL, "sched_setscheduler(0, SCHED_OTHER, 0) failed\n");
}
}
diff --git a/aos/linux_code/init.h b/aos/linux_code/init.h
index e069b83..e34ecfd 100644
--- a/aos/linux_code/init.h
+++ b/aos/linux_code/init.h
@@ -30,6 +30,9 @@
// Sets the current thread's realtime priority.
void SetCurrentThreadRealtimePriority(int priority);
+// Sets the current thread back down to non-realtime priority.
+void UnsetCurrentThreadRealtimePriority();
+
// Pins the current thread to CPU #number.
void PinCurrentThreadToCPU(int number);
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();
diff --git a/frc971/wpilib/ADIS16448.h b/frc971/wpilib/ADIS16448.h
index c8552bf..4f2aaa0 100644
--- a/frc971/wpilib/ADIS16448.h
+++ b/frc971/wpilib/ADIS16448.h
@@ -8,6 +8,7 @@
#include "SPI.h"
#include "DigitalInput.h"
+#include "DigitalOutput.h"
#undef ERROR
#include "aos/common/logging/logging.h"
@@ -29,6 +30,14 @@
// dio1 must be connected to DIO1 on the sensor.
ADIS16448(SPI::Port port, DigitalInput *dio1);
+ // Sets the dummy SPI port to send values on to make the roboRIO deassert the
+ // chip select line. This is mainly useful when there are no other devices
+ // sharing the bus.
+ void SetDummySPI(SPI::Port port);
+
+ // Sets the reset line for the IMU to use for error recovery.
+ void set_reset(DigitalOutput *output) { reset_ = output; }
+
// For ::std::thread to call.
//
// Initializes the sensor and then loops until Quit() is called taking
@@ -42,6 +51,9 @@
double gyro_z_zeroed_offset() const { return gyro_z_zeroed_offset_; }
private:
+ // Try to initialize repeatedly as long as we're supposed to be running.
+ void InitializeUntilSuccessful();
+
// Converts a 16-bit value at data to a scaled output value where a value of 1
// corresponds to lsb_per_output.
float ConvertValue(uint8_t *data, double lsb_per_output, bool sign = true);
@@ -70,8 +82,11 @@
// Returns true if it succeeds.
bool Initialize();
+ // TODO(Brian): This object has no business owning these ones.
const ::std::unique_ptr<SPI> spi_;
+ ::std::unique_ptr<SPI> dummy_spi_;
DigitalInput *const dio1_;
+ DigitalOutput *reset_ = nullptr;
::std::atomic<bool> run_{true};
diff --git a/y2017/wpilib_interface.cc b/y2017/wpilib_interface.cc
index 09d36a6..d13df96 100644
--- a/y2017/wpilib_interface.cc
+++ b/y2017/wpilib_interface.cc
@@ -605,6 +605,9 @@
auto imu_trigger = make_unique<DigitalInput>(3);
::frc971::wpilib::ADIS16448 imu(SPI::Port::kOnboardCS1, imu_trigger.get());
+ imu.SetDummySPI(SPI::Port::kOnboardCS2);
+ auto imu_reset = make_unique<DigitalOutput>(6);
+ imu.set_reset(imu_reset.get());
::std::thread imu_thread(::std::ref(imu));
DrivetrainWriter drivetrain_writer;